update code

mkr2018_mrc/robot_coordination/src/robot.cpp

@@ -18,10 +18,13 @@ Robot::Robot() :
   threshold_(0.1) {
 }
 
-void Robot::odometryCallback (const nav_msgs::Odometry::ConstPtr& msg) {
+void Robot::odometryCallback (const nav_msgs::Odometry::ConstPtr& msg) 
+{
   ROS_DEBUG("got odometry");
-    robot_pose_ = msg->pose.pose;
-  if (execute_) {
+  robot_pose_ = msg->pose.pose;
+  
+  if (execute_) 
+  {
     followTheCarrot();
   }
 }
@@ -86,79 +89,95 @@ bool Robot::followTheCarrot()
       command_publisher_->publish(command);
       return true;
     }
-    else {
-    while (!plan_.empty() && plan_.front().distanceTo(robot_pose_) < threshold_) {
-      plan_.pop_front();
-    }
-    
-    MoveBaseClient ac("/robot1/stdr_robot1_move_base", true);
-    while(!ac.waitForServer(ros::Duration(5.0)))
+    else 
     {
-      ROS_WARN("Waiting for the stdr_robot1_move_base action server to come up");
-    }
-
-    Waypoint actual_goal = plan_.front();
-    move_base_msgs::MoveBaseGoal goal;
-
-    goal.target_pose.header.frame_id = ros::this_node::getName();
-    goal.target_pose.header.stamp = ros::Time::now();
-
-    goal.target_pose.pose.position.x = actual_goal.pose.orientation.x;
-    goal.target_pose.pose.position.y = actual_goal.pose.orientation.y;
-    goal.target_pose.pose.orientation = tf::createQuaternionMsgFromYaw(M_PI);
-
-    ROS_INFO("Sending goal");
-    ac.sendGoal(goal);
-
-    ac.waitForResult();
-
-    if(ac.getState() == actionlib::SimpleClientGoalState::SUCCEEDED)
-      ROS_INFO("Move goal ok !");
-    else
-      ROS_WARN("Move goal failed !");
-
-    #if 0
-    const double dist = actual_goal.distanceTo(robot_pose_);
-    const double angle = actual_goal.angleTo(robot_pose_);
-    tf::Quaternion q(
-        robot_pose_.orientation.x,
-        robot_pose_.orientation.y,
-        robot_pose_.orientation.z,
-        robot_pose_.orientation.w);
-    tf::Matrix3x3 m(q);
-    double roll;
-    double pitch;
-    double yaw;
-    m.getRPY(roll, pitch, yaw);
-    ROS_INFO("distance %f and angle %f to goal", dist,angle);
-    ROS_INFO("robot roll %f , pitch %f yaw %f", roll, pitch, yaw);
-    geometry_msgs::Twist command;
-    double delta = angle - yaw;
-    if (delta > M_PI ) {
-      delta -= 2 * M_PI;
-    }
-    if (delta < -M_PI ) {
-      delta += 2 * M_PI;
-    }
-    ROS_INFO_STREAM(ros::Time::now() << " " << actual_goal.timepoint);
-    const double time_to_goal = ((execute_start_time_ + actual_goal.timepoint) - ros::Time::now()).toSec();
-
-    ROS_INFO("time to goal %f ", time_to_goal);
-    double speed;
-    if (time_to_goal > 0) {
-      speed = dist/time_to_goal ;
-    } else {
-     speed = 1;
-    }
-    command.angular.z = 2 * (sin(delta) - 0.333 * sin(3 * delta));
-    command.angular.z = delta;
-    command.linear.x = speed * (cos(delta) + 0.333 * cos(3 * delta));
-    if (command.linear.x < 0) {
-      command.linear.x = 0;
-    }
-    command_publisher_->publish(command);
-    #endif
+      while (!plan_.empty() && plan_.front().distanceTo(robot_pose_) < threshold_) 
+      {
+        plan_.pop_front();
+      }
+    
+      std::string nodeName = ros::this_node::getName();
+      
+      std::string actionClientName = "";
+      std::string::size_type idx;
+      idx = nodeName.find("robot0");
+      if(idx != std::string::npos)
+      {
+          actionClientName = "/robot0/move_base";
+      }
+      else
+      {
+          actionClientName = "/robot1/move_base";
+      }
+      ROS_WARN_STREAM("actionName: " << actionClientName);
+      MoveBaseClient ac(actionClientName, false);
+      while(!ac.waitForServer(ros::Duration(5.0)))
+      {
+        ROS_WARN("Waiting for the move_base action server to come up");
+      }
+
+      Waypoint actual_goal = plan_.front();
+      move_base_msgs::MoveBaseGoal goal;
+
+      goal.target_pose.header.frame_id = ros::this_node::getName();
+      goal.target_pose.header.stamp = ros::Time::now();
+
+      goal.target_pose.pose.position.x = actual_goal.pose.orientation.x;
+      goal.target_pose.pose.position.y = actual_goal.pose.orientation.y;
+      goal.target_pose.pose.orientation = tf::createQuaternionMsgFromYaw(M_PI);
+      ROS_WARN_STREAM("pose_x:"<<goal.target_pose.pose.position.x<<" , pose_y:"<<goal.target_pose.pose.position.y);
+      ROS_INFO("Sending goal");
+      ac.sendGoal(goal);
+
+      ac.waitForResult();
+
+      if(ac.getState() == actionlib::SimpleClientGoalState::SUCCEEDED)
+        ROS_INFO("Move goal ok !");
+      else
+        ROS_WARN("Move goal failed !");
+
+      #if 0
+      const double dist = actual_goal.distanceTo(robot_pose_);
+      const double angle = actual_goal.angleTo(robot_pose_);
+      tf::Quaternion q(
+          robot_pose_.orientation.x,
+          robot_pose_.orientation.y,
+          robot_pose_.orientation.z,
+          robot_pose_.orientation.w);
+      tf::Matrix3x3 m(q);
+      double roll;
+      double pitch;
+      double yaw;
+      m.getRPY(roll, pitch, yaw);
+      ROS_INFO("distance %f and angle %f to goal", dist,angle);
+      ROS_INFO("robot roll %f , pitch %f yaw %f", roll, pitch, yaw);
+      geometry_msgs::Twist command;
+      double delta = angle - yaw;
+      if (delta > M_PI ) {
+        delta -= 2 * M_PI;
+      }
+      if (delta < -M_PI ) {
+        delta += 2 * M_PI;
+      }
+      ROS_INFO_STREAM(ros::Time::now() << " " << actual_goal.timepoint);
+      const double time_to_goal = ((execute_start_time_ + actual_goal.timepoint) - ros::Time::now()).toSec();
+
+      ROS_INFO("time to goal %f ", time_to_goal);
+      double speed;
+      if (time_to_goal > 0) {
+        speed = dist/time_to_goal ;
+      } else {
+      speed = 1;
+      }
+      command.angular.z = 2 * (sin(delta) - 0.333 * sin(3 * delta));
+      command.angular.z = delta;
+      command.linear.x = speed * (cos(delta) + 0.333 * cos(3 * delta));
+      if (command.linear.x < 0) {
+        command.linear.x = 0;
+      }
+      command_publisher_->publish(command);
+      #endif
 
-    return false;
+      return false;
   }
 }

mkr2018_mrc/simulator_e130/launch/robots.launch

@@ -2,7 +2,7 @@
 
 <launch>
 
-	<node name="robot0_spawner" pkg="stdr_robot" type="robot_handler" args="add $(find turtlebot_stdr)/robot/turtlebot.yaml 1.0 1.0 0" /> 
-	<node name="robot1_spawner" pkg="stdr_robot" type="robot_handler" args="add $(find turtlebot_stdr)/robot/turtlebot.yaml 2.0 1.0 0" /> 
+	<node name="robot0_spawner" pkg="stdr_robot" type="robot_handler" args="add $(find turtlebot_stdr)/robot/turtlebot.yaml 1.0 1.0 0" output="screen"/> 
+	<node name="robot1_spawner" pkg="stdr_robot" type="robot_handler" args="add $(find turtlebot_stdr)/robot/turtlebot.yaml 2.0 1.0 0" output="screen"/> 
 
 </launch>

mkr2018_mrc/simulator_e130/launch/server.launch

@@ -3,8 +3,7 @@
 <launch>
 	<include file="$(find stdr_robot)/launch/robot_manager.launch" />
 
-	<node type="stdr_server_node" pkg="stdr_server" name="stdr_server" args="$(find simulator_e130)/maps/floorplan_e130_e129.yaml"/>
-
-	<node pkg="tf" type="static_transform_publisher" name="world2map" args="0 0 0 0 0 0  world map 100" />
-
+	<node type="stdr_server_node" pkg="stdr_server" name="stdr_server" args="$(find simulator_e130)/maps/floorplan_e130_e129.yaml" output="screen"/>
+	
+	<node pkg="tf" type="static_transform_publisher" name="world2map" args="0 0 0 0 0 0  world /map 100" output="screen"/>
 </launch>

mkr2018_mrc/simulator_e130/launch/simulator.launch

@@ -8,6 +8,7 @@
     <!-- startup multi robots control node -->
     <include file="$(find robot_coordination)/launch/startup_robots_control.launch" />
 
+
     <!-- load map show in rviz -->
     <group ns="/robot0">
         <node pkg="map_server" type="map_server" name="stdr_load_map0" args="$(find simulator_e130)/maps/floorplan_e130_e129.yaml">

mkr2018_mrc/stdr_amcl/launch/stdr_amcl.launch

@@ -40,7 +40,7 @@
       <param name="initial_cov_aa" value="0.068538917"/> <!-- pi/12 * pi/12 -->
       <param name="gui_publish_rate" value="-1.0"/>
       <param name="save_pose_rate"   value="0.5"/>
-      <param name="use_map_topic"    value="false"/>
+      <param name="use_map_topic"    value="true"/>
       <param name="first_map_only"   value="false"/>
 
       <!-- laser model parameters -->
@@ -66,7 +66,7 @@
       <param name="odom_frame_id"   value="/map_static"/>
       <param name="base_frame_id"   value="/robot0"/>
       <param name="global_frame_id" value="/map"/>
-      <param name="tf_broadcast"    value="true"/>
+      <param name="tf_broadcast"    value="false"/>
     </node>
   </group>
 
@@ -99,7 +99,7 @@
       <param name="initial_cov_aa" value="0.068538917"/> <!-- pi/12 * pi/12 -->
       <param name="gui_publish_rate" value="-1.0"/>
       <param name="save_pose_rate"   value="0.5"/>
-      <param name="use_map_topic"    value="false"/>
+      <param name="use_map_topic"    value="true"/>
       <param name="first_map_only"   value="false"/>
 
       <!-- laser model parameters -->
@@ -125,7 +125,7 @@
       <param name="odom_frame_id"   value="/map_static"/>
       <param name="base_frame_id"   value="/robot1"/>
       <param name="global_frame_id" value="/map"/>
-      <param name="tf_broadcast"    value="true"/>
+      <param name="tf_broadcast"    value="false"/>
     </node>
   </group>
 

mkr2018_mrc/stdr_amcl/rviz/stdr_amcl_config.rviz

@@ -6,6 +6,7 @@ Panels:
       Expanded:
         - /Odometry1/Covariance1
         - /Odometry2/Covariance1
+        - /Map1/Status1
       Splitter Ratio: 0.522222221
     Tree Height: 595
   - Class: rviz/Selection
@@ -101,9 +102,9 @@ Visualization Manager:
       Enabled: true
       Invert Rainbow: true
       Max Color: 255; 255; 255
-      Max Intensity: 6.17128325
+      Max Intensity: 5.81653309
       Min Color: 0; 0; 0
-      Min Intensity: 2.53456712
+      Min Intensity: 2.19624114
       Name: LaserScan
       Position Transformer: XYZ
       Queue Size: 10
@@ -178,16 +179,6 @@ Visualization Manager:
       Topic: /robot1/particlecloud
       Unreliable: false
       Value: true
-    - Alpha: 0.699999988
-      Class: rviz/Map
-      Color Scheme: costmap
-      Draw Behind: true
-      Enabled: true
-      Name: Map
-      Topic: /amcl/map
-      Unreliable: false
-      Use Timestamp: false
-      Value: true
     - Alpha: 0.600000024
       Buffer Length: 20
       Class: rviz/Path
@@ -325,22 +316,6 @@ Visualization Manager:
       Topic: /robot1/odom
       Unreliable: false
       Value: true
-    - Alpha: 1
-      Class: rviz/Polygon
-      Color: 255; 24; 105
-      Enabled: true
-      Name: Polygon
-      Topic: /stdr_move_base/local_costmap/footprint
-      Unreliable: false
-      Value: true
-    - Alpha: 1
-      Class: rviz/Polygon
-      Color: 25; 255; 0
-      Enabled: true
-      Name: Polygon
-      Topic: ""
-      Unreliable: false
-      Value: true
     - Alpha: 0.699999988
       Axes Length: 1
       Axes Radius: 0.100000001
@@ -353,7 +328,7 @@ Visualization Manager:
       Shaft Length: 0.5
       Shaft Radius: 0.0500000007
       Shape: Arrow
-      Topic: /robot0/stdr_robot0_move_base/current_goal
+      Topic: /robot0/move_base/current_goal
       Unreliable: false
       Value: true
     - Alpha: 1
@@ -368,7 +343,7 @@ Visualization Manager:
       Shaft Length: 1
       Shaft Radius: 0.0500000007
       Shape: Arrow
-      Topic: /robot1/stdr_robot1_move_base/current_goal
+      Topic: /robot1/move_base/current_goal
       Unreliable: false
       Value: true
     - Alpha: 0.400000006
@@ -381,16 +356,6 @@ Visualization Manager:
       Unreliable: false
       Use Timestamp: false
       Value: true
-    - Alpha: 0.300000012
-      Class: rviz/Map
-      Color Scheme: map
-      Draw Behind: false
-      Enabled: true
-      Name: Map
-      Topic: /amcl/map
-      Unreliable: false
-      Use Timestamp: false
-      Value: true
   Enabled: true
   Global Options:
     Background Color: 48; 48; 48
@@ -434,7 +399,7 @@ Visualization Manager:
       Pitch: 1.33479989
       Target Frame: <Fixed Frame>
       Value: Orbit (rviz)
-      Yaw: 4.78319407
+      Yaw: 4.70819187
     Saved: ~
 Window Geometry:
   Displays:

mkr2018_mrc/stdr_move_base/config/local_costmap_params0.yaml

@@ -14,7 +14,7 @@ local_costmap:
   robot_base_frame: /robot0
   update_frequency: 5.0
   publish_frequency: 3.0
-  static_map: false
+  static_map: true
   rolling_window: true
   width: 4.0
   height: 4.0

mkr2018_mrc/stdr_move_base/config/local_costmap_params1.yaml

@@ -14,7 +14,7 @@ local_costmap:
   robot_base_frame: /robot1
   update_frequency: 5.0
   publish_frequency: 3.0
-  static_map: false
+  static_map: true
   rolling_window: true
   width: 4.0
   height: 4.0

mkr2018_mrc/stdr_move_base/launch/stdr_move_base.launch

@@ -14,11 +14,11 @@
     <arg name="base_frame_id"   default="/robot0"/>
     <arg name="global_frame_id" default="/map"/>
 
-    <arg name="odom_topic"      default="/robot0/odom"/>
-    <arg name="cmd_vel_topic"   default="/robot0/cmd_vel"/>
-    <arg name="map_topic"       default="/amcl/map"/>
+    <arg name="odom_topic"      default="/robot0/odom" />
+    <arg name="cmd_vel_topic"   default="/robot0/cmd_vel" />
+    <arg name="map_topic"       default="/amcl/map" />
 
-    <node pkg="move_base" type="move_base" name="stdr_robot0_move_base" output="screen">
+    <node pkg="move_base" type="move_base" name="move_base" output="screen">
       <rosparam file="$(find stdr_move_base)/config/costmap_common_params0.yaml" command="load" ns="global_costmap" />
       <rosparam file="$(find stdr_move_base)/config/costmap_common_params0.yaml" command="load" ns="local_costmap" />
       <rosparam file="$(find stdr_move_base)/config/local_costmap_params0.yaml"  command="load" />
@@ -45,15 +45,16 @@
   </group>
 
   <group ns="/robot1">
-    <arg name="odom_frame_id"   default="/map_static"/>
-    <arg name="base_frame_id"   default="/robot1"/>
-    <arg name="global_frame_id" default="/map"/>
+    <arg name="odom_frame_id"    default="/map_static"/>
+    <arg name="base_frame_id"    default="/robot1"/>
+    <arg name="global_frame_id"  default="/map"/>
+
+    <arg name="odom_topic"       default="/robot1/odom"/>
+    <arg name="cmd_vel_topic"    default="/robot1/cmd_vel"/>
+    <arg name="map_topic"        default="/amcl/map"/>
 
-    <arg name="odom_topic"      default="/robot1/odom"/>
-    <arg name="cmd_vel_topic"   default="/robot1/cmd_vel"/>
-    <arg name="map_topic"       default="/amcl/map"/>
 
-    <node pkg="move_base" type="move_base" name="stdr_robot1_move_base" output="screen">
+    <node pkg="move_base" type="move_base" name="move_base" output="screen">
       <rosparam file="$(find stdr_move_base)/config/costmap_common_params1.yaml" command="load" ns="global_costmap" />
       <rosparam file="$(find stdr_move_base)/config/costmap_common_params1.yaml" command="load" ns="local_costmap" />
       <rosparam file="$(find stdr_move_base)/config/local_costmap_params1.yaml"  command="load" />