contains "nmpc_solver_setup_mrp_hex_shadow.m" for switching to shadow set

mav_control_interface/include/mav_control_interface/position_controller_interface.h

@@ -51,6 +51,8 @@ class PositionControllerInterface {
       mav_msgs::EigenRollPitchYawrateThrust* attitude_thrust_command);
 
   virtual bool calculateAttitudeThrustCommand(mav_msgs::EigenAttitudeThrust* attitude_thrust_command);
+
+  virtual bool calculateForcesCommand(mav_msgs::Actuators* forces_command);
 };
 
 } /* namespace mav_flight_manager */

mav_control_interface/src/mav_control_interface_impl.cpp

@@ -19,6 +19,7 @@
 #include <mav_msgs/default_topics.h>
 #include <mav_msgs/AttitudeThrust.h>
 #include <mav_msgs/RollPitchYawrateThrust.h>
+#include <mav_msgs/Actuators.h>
 #include <trajectory_msgs/MultiDOFJointTrajectory.h>
 
 #include "mav_control_interface_impl.h"
@@ -72,7 +73,7 @@ MavControlInterfaceImpl::MavControlInterfaceImpl(ros::NodeHandle& nh, ros::NodeH
     ROS_WARN("rc_teleop_max_carrot_distance_yaw_ was limited to pi/2. This is by far enough.");
   }
 
-  interface_nh.param("rc_max_roll_pitch_command", p.rc_max_roll_pitch_command_,
+  interface_nh.param("rc_max_roll_pitch_command", p.rc_max_roll_pitch_command_, 
                      Parameters::kDefaultRcMaxRollPitchCommand);
   interface_nh.param("rc_max_yaw_rate_command", p.rc_max_yaw_rate_command_,
                      Parameters::kDefaultRcMaxYawRateCommand);
@@ -101,7 +102,7 @@ void MavControlInterfaceImpl::RcUpdatedCallback(const RcInterfaceBase& rc_interf
 
 void MavControlInterfaceImpl::CommandPoseCallback(const geometry_msgs::PoseStampedConstPtr& msg)
 {
-
+  //ROS_INFO_STREAM("command pose callback");
   mav_msgs::EigenTrajectoryPoint reference;
   mav_msgs::eigenTrajectoryPointFromPoseMsg(*msg, &reference);
 

mav_control_interface/src/mav_control_interface_impl.h

@@ -69,7 +69,8 @@ class MavControlInterfaceImpl
   bool TakeoffCallback(std_srvs::EmptyRequest& request, std_srvs::EmptyResponse& response);
   bool BackToPositionHoldCallback(std_srvs::Empty::Request& request, std_srvs::Empty::Response& response);
 
-  void publishAttitudeCommand(const mav_msgs::RollPitchYawrateThrust& command);
+ // void publishAttitudeCommand(const mav_msgs::RollPitchYawrateThrust& command);
+ // void publishForcesCommand(const mav_msgs::Actuators& command);
 };
 
 } /* namespace mav_control_interface */

mav_control_interface/src/position_controller_interface.cpp

@@ -49,4 +49,10 @@ bool PositionControllerInterface::calculateAttitudeThrustCommand(
   return false;
 }
 
+bool PositionControllerInterface::calculateForcesCommand(mav_msgs::Actuators* forces_command) {
+	ROS_WARN_STREAM_THROTTLE(1, "calculateForcesCommand() not implemented for controller "
+		<< this->getName());
+	return false;
+}
+
 } /* namespace mav_flight_manager */

mav_control_interface/src/state_machine.cpp

@@ -34,6 +34,10 @@ StateMachineDefinition::StateMachineDefinition(const ros::NodeHandle& nh, const
   command_publisher_ = nh_.advertise<mav_msgs::RollPitchYawrateThrust>(
       mav_msgs::default_topics::COMMAND_ROLL_PITCH_YAWRATE_THRUST, 1);
 
+  forces_command_publisher_ = nh_.advertise<mav_msgs::Actuators>(
+		  "command/motor_speed" , 1);
+
+
   current_reference_publisher_ = nh_.advertise<trajectory_msgs::MultiDOFJointTrajectory>(
       "command/current_reference", 1);
 
@@ -42,8 +46,6 @@ StateMachineDefinition::StateMachineDefinition(const ros::NodeHandle& nh, const
   private_nh_.param<bool>("use_rc_teleop", use_rc_teleop_, true);
   private_nh_.param<std::string>("reference_frame", reference_frame_id_, "odom");
   predicted_state_publisher_ = nh_.advertise<visualization_msgs::Marker>( "predicted_state", 0 );
-  full_predicted_state_publisher_ = 
-    nh_.advertise<trajectory_msgs::MultiDOFJointTrajectory>( "full_predicted_state", 1 );
 }
 
 void StateMachineDefinition::SetParameters(const Parameters& parameters)
@@ -63,9 +65,19 @@ void StateMachineDefinition::PublishAttitudeCommand (
 
   msg->header.stamp = ros::Time::now();  // TODO(acmarkus): get from msg
   mav_msgs::msgRollPitchYawrateThrustFromEigen(command, msg.get());
-  command_publisher_.publish(msg);
+ command_publisher_.publish(msg);
+}
+
+
+void StateMachineDefinition::PublishForcesCommand(const mav_msgs::Actuators& command_msg) const
+{
+	mav_msgs::Actuators msg = command_msg;
+	msg.header.stamp = ros::Time::now();
+	forces_command_publisher_.publish(msg);
+
 }
 
+
 void StateMachineDefinition::PublishStateInfo(const std::string& info)
 {
   if (state_info_publisher_.getNumSubscribers() > 0) {
@@ -129,21 +141,6 @@ void StateMachineDefinition::PublishPredictedState()
     predicted_state_publisher_.publish(marker_queue);
   }
 
-  if (full_predicted_state_publisher_.getNumSubscribers() > 0) {
-    mav_msgs::EigenTrajectoryPointDeque predicted_state;
-    controller_->getPredictedState(&predicted_state);
-
-    trajectory_msgs::MultiDOFJointTrajectory msg;
-    msgMultiDofJointTrajectoryFromEigen(predicted_state, &msg);
-
-    //add in timestamp information
-    if (!predicted_state.empty()) {
-      msg.header.stamp.fromNSec(predicted_state.front().timestamp_ns -
-                         predicted_state.front().time_from_start_ns);
-    }
-
-    full_predicted_state_publisher_.publish(msg);
-  }
 }
 
 } // end namespace state_machine

mav_control_interface/src/state_machine.cpp~

@@ -0,0 +1,151 @@
+/*
+* Copyright (c) 2015, Markus Achtelik, ASL, ETH Zurich, Switzerland
+* You can contact the author at <markus dot achtelik at mavt dot ethz dot ch>
+*
+* Licensed under the Apache License, Version 2.0 (the "License");
+* you may not use this file except in compliance with the License.
+* You may obtain a copy of the License at
+*
+* http://www.apache.org/licenses/LICENSE-2.0
+*
+* Unless required by applicable law or agreed to in writing, software
+* distributed under the License is distributed on an "AS IS" BASIS,
+* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+* See the License for the specific language governing permissions and
+* limitations under the License.
+*/
+
+#include <mav_msgs/default_topics.h>
+#include <std_msgs/String.h>
+#include <tf_conversions/tf_eigen.h>
+
+#include "state_machine.h"
+
+namespace mav_control_interface {
+
+namespace state_machine {
+
+StateMachineDefinition::StateMachineDefinition(const ros::NodeHandle& nh, const ros::NodeHandle& private_nh,
+                                               std::shared_ptr<PositionControllerInterface> controller)
+    :nh_(nh),
+     private_nh_(private_nh),
+     controller_(controller)
+{
+  command_publisher_ = nh_.advertise<mav_msgs::RollPitchYawrateThrust>(
+      mav_msgs::default_topics::COMMAND_ROLL_PITCH_YAWRATE_THRUST, 1);
+
+  forces_command_publisher_ = nh_.advertise<mav_msgs::Actuators>(
+	  mav_msgs::default_topics::COMMAND_ACTUATORS, 1);
+
+  current_reference_publisher_ = nh_.advertise<trajectory_msgs::MultiDOFJointTrajectory>(
+      "command/current_reference", 1);
+
+  state_info_publisher_ = nh_.advertise<std_msgs::String>("state_machine/state_info", 1, true);
+
+  private_nh_.param<bool>("use_rc_teleop", use_rc_teleop_, true);
+  private_nh_.param<std::string>("reference_frame", reference_frame_id_, "odom");
+  predicted_state_publisher_ = nh_.advertise<visualization_msgs::Marker>( "predicted_state", 0 );
+}
+
+void StateMachineDefinition::SetParameters(const Parameters& parameters)
+{
+  parameters_ = parameters;
+}
+
+void StateMachineDefinition::PublishAttitudeCommand (
+    const mav_msgs::EigenRollPitchYawrateThrust& command) const
+{
+  mav_msgs::RollPitchYawrateThrustPtr msg(new mav_msgs::RollPitchYawrateThrust);
+
+  mav_msgs::EigenRollPitchYawrateThrust tmp_command = command;
+  tmp_command.thrust.x() = 0;
+  tmp_command.thrust.y() = 0;
+  tmp_command.thrust.z() = std::max(0.0, command.thrust.z());
+
+  msg->header.stamp = ros::Time::now();  // TODO(acmarkus): get from msg
+  mav_msgs::msgRollPitchYawrateThrustFromEigen(command, msg.get());
+  command_publisher_.publish(msg);
+}
+
+void StateMachineDefinition::PublishForcesCommand(
+	const mav_msgs::EigenActuators& command) const
+{
+	mav_msgs::ActuatorsPtr msg(new mav_msgs::Actuators);
+
+	msg->header.stamp = ros::Time::now();  
+	mav_msgs::msgActuatorsFromEigen(command, msg.get());
+	msg->angular_velocities.clear();
+	forces_command_publisher_.publish(msg);
+}
+
+
+
+
+void StateMachineDefinition::PublishStateInfo(const std::string& info)
+{
+  if (state_info_publisher_.getNumSubscribers() > 0) {
+    std_msgs::StringPtr msg(new std_msgs::String);
+    msg->data = info;
+    state_info_publisher_.publish(msg);
+  }
+}
+
+void StateMachineDefinition::PublishCurrentReference()
+{
+  ros::Time time_now = ros::Time::now();
+  mav_msgs::EigenTrajectoryPoint current_reference;
+  controller_->getCurrentReference(&current_reference);
+
+  tf::Quaternion q;
+  tf::Vector3 p;
+  tf::vectorEigenToTF(current_reference.position_W, p);
+  tf::quaternionEigenToTF(current_reference.orientation_W_B, q);
+
+  tf::Transform transform;
+  transform.setOrigin(p);
+  transform.setRotation(q);
+
+  transform_broadcaster_.sendTransform(
+      tf::StampedTransform(transform, time_now, reference_frame_id_, nh_.getNamespace() + "/current_reference"));
+
+  if (current_reference_publisher_.getNumSubscribers() > 0) {
+    trajectory_msgs::MultiDOFJointTrajectoryPtr msg(new trajectory_msgs::MultiDOFJointTrajectory);
+    mav_msgs::msgMultiDofJointTrajectoryFromEigen(current_reference, msg.get());
+    msg->header.stamp = time_now;
+    msg->header.frame_id = reference_frame_id_;
+    current_reference_publisher_.publish(msg);
+  }
+}
+
+void StateMachineDefinition::PublishPredictedState()
+{
+  if (predicted_state_publisher_.getNumSubscribers() > 0) {
+    mav_msgs::EigenTrajectoryPointDeque predicted_state;
+    controller_->getPredictedState(&predicted_state);
+    visualization_msgs::Marker marker_queue;
+    marker_queue.header.frame_id = reference_frame_id_;
+    marker_queue.header.stamp = ros::Time();
+    marker_queue.type = visualization_msgs::Marker::LINE_STRIP;
+    marker_queue.scale.x = 0.05;
+    marker_queue.color.a = 1.0;
+    marker_queue.color.r = 1.0;
+
+    //    marker_heading.type = visualization_msgs::Marker::ARROW;
+    {
+      for (size_t i = 0; i < predicted_state.size(); i++) {
+        geometry_msgs::Point p;
+        p.x = predicted_state.at(i).position_W(0);
+        p.y = predicted_state.at(i).position_W(1);
+        p.z = predicted_state.at(i).position_W(2);
+        marker_queue.points.push_back(p);
+      }
+    }
+
+    predicted_state_publisher_.publish(marker_queue);
+  }
+
+}
+
+} // end namespace state_machine
+
+} // namespace mav_control_interface

mav_control_interface/src/state_machine.h

@@ -35,6 +35,7 @@
 #include <mav_msgs/eigen_mav_msgs.h>
 #include <ros/ros.h>
 #include <tf/transform_broadcaster.h>
+#include <Eigen/Eigen>
 
 #include <mav_control_interface/position_controller_interface.h>
 #include <mav_control_interface/rc_interface.h>
@@ -144,6 +145,8 @@ class StateMachineDefinition : public msm_front::state_machine_def<StateMachineD
 
   // Now define transition table:
   struct transition_table : boost::mpl::vector<
+
+     //msm_front::Row<PositionHold, OdometryUpdate, InternalTransition, SetOdometryAndCompute, NoGuard>
       //    Start     Event         Next      Action                     Guard
       //  +---------+-------------+---------+---------------------------+----------------------+
       msm_front::Row<Inactive, RcUpdate, RemoteControl, NoAction, euml::And_<RcModeManual, RcOn> >,
@@ -208,12 +211,12 @@ class StateMachineDefinition : public msm_front::state_machine_def<StateMachineD
   std::string reference_frame_id_;
   std::shared_ptr<PositionControllerInterface> controller_;
   ros::Publisher command_publisher_;
+  ros::Publisher forces_command_publisher_;
   ros::Publisher state_info_publisher_;
 
   tf::TransformBroadcaster transform_broadcaster_;
   ros::Publisher current_reference_publisher_;
   ros::Publisher predicted_state_publisher_;
-  ros::Publisher full_predicted_state_publisher_;
   Parameters parameters_;
   mav_msgs::EigenOdometry current_state_;
   mav_msgs::EigenTrajectoryPointDeque current_reference_queue_;
@@ -222,6 +225,7 @@ class StateMachineDefinition : public msm_front::state_machine_def<StateMachineD
   void PublishStateInfo(const std::string& info);
   void PublishCurrentReference();
   void PublishPredictedState();
+  void PublishForcesCommand(const mav_msgs::Actuators& command) const;
 
   // Implementation of state machine:
 
@@ -341,14 +345,20 @@ class StateMachineDefinition : public msm_front::state_machine_def<StateMachineD
     template<class EVT, class FSM, class SourceState, class TargetState>
     void operator()(EVT const& evt, FSM& fsm, SourceState&, TargetState&)
     {
-      mav_msgs::EigenRollPitchYawrateThrust command;
-      fsm.controller_->calculateRollPitchYawrateThrustCommand(&command);
-      fsm.PublishAttitudeCommand(command);
+      //ROS_INFO_STREAM("We are in Compute Command ");
+      //mav_msgs::EigenRollPitchYawrateThrust command;
+      //fsm.controller_->calculateRollPitchYawrateThrustCommand(&command);
+      //fsm.PublishAttitudeCommand(command);
+
+   	  mav_msgs::Actuators turning_velocities_msg;
+      fsm.controller_->calculateForcesCommand(&turning_velocities_msg);
+      fsm.PublishForcesCommand(turning_velocities_msg);
       fsm.PublishCurrentReference();
       fsm.PublishPredictedState();
     }
   };
 
+
   struct SetReferenceFromRc
   {
     double YawWrap(double yaw)