Skip to content

Commit a658fb9

Browse files
globeofgeekglobeofgeek
committed
Multi robot simulation works with arc_stage. Fixed the issue requiring 2 launch files. Stage modified to publish tf transforms WITHOUT /arc prefix. Consequence now, is that planning must be done in /map frame, not /base_link, so random wander schema will need an update to support different frames.
1 parent 1eff027 commit a658fb9

File tree

5 files changed

+150
-86
lines changed

5 files changed

+150
-86
lines changed

‎arc_behaviour/src/NavigationAdapter.cpp‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -73,7 +73,7 @@ bool NavigationAdapter::move_to_goal_request_cb(arc_msgs::NavigationRequest::Req
7373
void NavigationAdapter::sendGoal(arc_msgs::NavigationRequest::Request &req) {
7474
move_base_msgs::MoveBaseGoal goal;
7575

76-
goal.target_pose.header.frame_id = "base_link";
76+
goal.target_pose.header.frame_id = "map";
7777
goal.target_pose.header.stamp = ros::Time::now();
7878

7979
//setting goal information from request.

‎arc_control/launch/testing.launch‎

Lines changed: 59 additions & 29 deletions
Original file line numberDiff line numberDiff line change
@@ -1,32 +1,62 @@
11
<launch>
2-
<!-- All content of ARC will be contained in the /arc namespace -->
3-
<group ns="arc">
4-
<group ns="test_bot">
5-
<include file="$(find arc_behaviour)/launch/default.launch" />
6-
7-
<!-- NAVIGATION SECTION -->
8-
<master auto="start"/>
9-
<param name="/use_sim_time" value="true"/>
10-
<include file="$(find arc_navigation)/move_base_config/move_base.xml"/>
11-
<node name="map_server" pkg="map_server" type="map_server" args="$(find arc_stage)/maps/config/arc_small.yaml" respawn="false" />
12-
<node name="fake_localization" pkg="fake_localization" type="fake_localization" respawn="false" />
13-
<node name="rviz" pkg="rviz" type="rviz" args="-d $(find arc_navigation)/single_robot.rviz" />
14-
15-
<!-- ARC_STAGE SECTION -->
16-
<param name="/use_sim_time" value="true"/>
17-
<node pkg="arc_stage" type="arc_stage" name="arc_stage" args="$(find arc_stage)/world/test_random_wander_ms.world" respawn="false" >
18-
<param name="base_watchdog_timeout" value="0.2"/>
19-
</node>
20-
</group>
21-
</group>
22-
</launch>
2+
<master auto="start"/>
3+
<param name="/use_sim_time" value="true"/>
4+
<node pkg="map_server" type="map_server" name="map_server" args="$(find arc_stage)/maps/config/arc_small.yaml" respawn="false" >
5+
<param name="frame_id" value="/map" />
6+
</node>
7+
8+
9+
<group ns="arc">
10+
<node pkg="arc_stage" type="arc_stage" name="arc_stage" args="-u $(find arc_stage)/world/test_multi_robot.world" respawn="false">
11+
<param name="base_watchdog_timeout" value="0.2"/>
12+
</node>
13+
<!-- BEGIN ROBOT 0 -->
14+
<group ns="test_bot">
15+
16+
<!-- BEHAVIOUR FOR ROBOT 0 -->
17+
<include file="$(find arc_behaviour)/launch/default.launch" />
18+
19+
<param name="tf_prefix" value="test_bot" />
2320

24-
<!-- open a terminal, and
25-
source /opt/ros/kinetic/setup.bash
26-
source /home/votick/workspace/arc_ws/src
27-
roslaunch arc_control testing.launch
21+
<node pkg="fake_localization" type="fake_localization" name="fake_localization" respawn="false" output="screen">
22+
<param name="tf_prefix" value="test_bot" />
23+
<param name="odom_frame_id" value="test_bot/odom" />
24+
<param name="base_frame_id" value="test_bot/base_link" />
25+
</node>
26+
<node pkg="move_base" type="move_base" respawn="false" name="move_base_node" output="screen">
27+
<remap from="map" to="/map" />
28+
<param name="controller_frequency" value="10.0" />
29+
<rosparam file="$(find navigation_stage)/move_base_config/costmap_common_params.yaml" command="load" ns="global_costmap" />
30+
<rosparam file="$(find navigation_stage)/move_base_config/costmap_common_params.yaml" command="load" ns="local_costmap" />
31+
<rosparam file="$(find navigation_stage)/move_base_config/local_costmap_params.yaml" command="load" />
32+
<rosparam file="$(find navigation_stage)/move_base_config/global_costmap_params.yaml" command="load" />
33+
<rosparam file="$(find navigation_stage)/move_base_config/base_local_planner_params.yaml" command="load" />
34+
</node>
35+
</group>
36+
<!-- END ROBOT 0 -->
2837

29-
This works fine on willow map.
30-
This works on custom random_wander_ms.world
31-
Using local arc_stage parameters for navigation stack [YES]
32-
-->
38+
<!-- BEGIN ROBOT 1 -->
39+
<group ns="test_bot_2">
40+
<param name="tf_prefix" value="test_bot_2" />
41+
42+
<node pkg="move_base" type="move_base" respawn="false" name="move_base_node" output="screen">
43+
<remap from="map" to="/map" />
44+
<param name="controller_frequency" value="10.0" />
45+
<rosparam file="$(find navigation_stage)/move_base_config/costmap_common_params.yaml" command="load" ns="global_costmap" />
46+
<rosparam file="$(find navigation_stage)/move_base_config/costmap_common_params.yaml" command="load" ns="local_costmap" />
47+
<rosparam file="$(find navigation_stage)/move_base_config/local_costmap_params.yaml" command="load" />
48+
<rosparam file="$(find navigation_stage)/move_base_config/global_costmap_params.yaml" command="load" />
49+
<rosparam file="$(find navigation_stage)/move_base_config/base_local_planner_params.yaml" command="load" />
50+
</node>
51+
52+
<node pkg="fake_localization" type="fake_localization" name="fake_localization" respawn="false">
53+
<param name="odom_frame_id" value="test_bot_2/odom" />
54+
<param name="base_frame_id" value="test_bot_2/base_link" />
55+
</node>
56+
</group>
57+
<!-- END ROBOT 1 -->
58+
59+
<node name="rviz" pkg="rviz" type="rviz" args="-d $(find navigation_stage)/multi_robot.rviz" />
60+
61+
</group>
62+
</launch>

‎arc_control/launch/weird_nav_hack.launch‎

Lines changed: 0 additions & 27 deletions
This file was deleted.

‎arc_navigation/single_robot.rviz‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -35,7 +35,7 @@ Visualization Manager:
3535
Draw Behind: false
3636
Enabled: true
3737
Name: Map
38-
Topic: /arc/test_bot/map
38+
Topic: /map
3939
Value: true
4040
- Alpha: 1
4141
Class: rviz/Polygon

‎arc_stage/src/stageros.cpp‎

Lines changed: 89 additions & 28 deletions
Original file line numberDiff line numberDiff line change
@@ -152,6 +152,10 @@ class StageNode
152152
const char *mapName(const char *name, size_t robotID, Stg::Model* mod) const;
153153
const char *mapName(const char *name, size_t robotID, size_t deviceID, Stg::Model* mod) const;
154154

155+
//Append robot name to a tf message. This is for multi-robot simulation with arc.
156+
const char *mapTfName(const char *name, size_t robotID, Stg::Model* mod) const;
157+
const char *mapTfName(const char *name, size_t robotID, size_t deviceID, Stg::Model* mod) const;
158+
155159
tf::TransformBroadcaster tf;
156160

157161
// Last time that we received a velocity command
@@ -203,6 +207,33 @@ class StageNode
203207
Stg::World* world;
204208
};
205209

210+
// since stageros is single-threaded, this is OK. revisit if that changes!
211+
const char *
212+
StageNode::mapTfName(const char *name, size_t robotID, Stg::Model* mod) const
213+
{
214+
//ROS_INFO("Robot %lu: Device %s", robotID, name);
215+
bool umn = this->use_model_names;
216+
217+
if ((positionmodels.size() > 1 ) || umn)
218+
{
219+
static char buf[100];
220+
std::size_t found = std::string(((Stg::Ancestor *) mod)->Token()).find(":");
221+
222+
if ((found==std::string::npos) && umn)
223+
{
224+
snprintf(buf, sizeof(buf), "/%s/%s", ((Stg::Ancestor *) mod)->Token(), name);
225+
}
226+
else
227+
{
228+
snprintf(buf, sizeof(buf), "/robot_%u/%s", (unsigned int)robotID, name);
229+
}
230+
231+
return buf;
232+
}
233+
else
234+
return name;
235+
}
236+
206237
// since stageros is single-threaded, this is OK. revisit if that changes!
207238
const char *
208239
StageNode::mapName(const char *name, size_t robotID, Stg::Model* mod) const
@@ -275,6 +306,36 @@ StageNode::mapName(const char *name, size_t robotID, size_t deviceID, Stg::Model
275306
}
276307
}
277308

309+
const char *
310+
StageNode::mapTfName(const char *name, size_t robotID, size_t deviceID, Stg::Model* mod) const
311+
{
312+
//ROS_INFO("Robot %lu: Device %s:%lu", robotID, name, deviceID);
313+
bool umn = this->use_model_names;
314+
315+
if ((positionmodels.size() > 1 ) || umn)
316+
{
317+
static char buf[100];
318+
std::size_t found = std::string(((Stg::Ancestor *) mod)->Token()).find(":");
319+
320+
if ((found==std::string::npos) && umn)
321+
{
322+
snprintf(buf, sizeof(buf), "/%s/%s_%u", ((Stg::Ancestor *) mod)->Token(), name, (unsigned int)deviceID);
323+
}
324+
else
325+
{
326+
snprintf(buf, sizeof(buf), "/robot_%u/%s_%u", (unsigned int)robotID, name, (unsigned int)deviceID);
327+
}
328+
329+
return buf;
330+
}
331+
else
332+
{
333+
static char buf[100];
334+
snprintf(buf, sizeof(buf), "/%s_%u", name, (unsigned int)deviceID);
335+
return buf;
336+
}
337+
}
338+
278339
void
279340
StageNode::ghfunc(Stg::Model* mod, StageNode* node)
280341
{
@@ -628,9 +689,9 @@ StageNode::WorldCallback()
628689
}
629690

630691
if (robotmodel->lasermodels.size() > 1)
631-
msg.header.frame_id = mapName(BASE_LASER_LINK, r, s, static_cast<Stg::Model*>(robotmodel->positionmodel));
692+
msg.header.frame_id = mapTfName(BASE_LASER_LINK, r, s, static_cast<Stg::Model*>(robotmodel->positionmodel));
632693
else
633-
msg.header.frame_id = mapName(BASE_LASER_LINK, r, static_cast<Stg::Model*>(robotmodel->positionmodel));
694+
msg.header.frame_id = mapTfName(BASE_LASER_LINK, r, static_cast<Stg::Model*>(robotmodel->positionmodel));
634695

635696
msg.header.stamp = sim_time;
636697
robotmodel->laser_pubs[s].publish(msg);
@@ -645,12 +706,12 @@ StageNode::WorldCallback()
645706

646707
if (robotmodel->lasermodels.size() > 1)
647708
tf.sendTransform(tf::StampedTransform(txLaser, sim_time,
648-
mapName(BASE_LINK, r, static_cast<Stg::Model*>(robotmodel->positionmodel)),
649-
mapName(BASE_LASER_LINK, r, s, static_cast<Stg::Model*>(robotmodel->positionmodel))));
709+
mapTfName(BASE_LINK, r, static_cast<Stg::Model*>(robotmodel->positionmodel)),
710+
mapTfName(BASE_LASER_LINK, r, s, static_cast<Stg::Model*>(robotmodel->positionmodel))));
650711
else
651712
tf.sendTransform(tf::StampedTransform(txLaser, sim_time,
652-
mapName(BASE_LINK, r, static_cast<Stg::Model*>(robotmodel->positionmodel)),
653-
mapName(BASE_LASER_LINK, r, static_cast<Stg::Model*>(robotmodel->positionmodel))));
713+
mapTfName(BASE_LINK, r, static_cast<Stg::Model*>(robotmodel->positionmodel)),
714+
mapTfName(BASE_LASER_LINK, r, static_cast<Stg::Model*>(robotmodel->positionmodel))));
654715
}
655716

656717
//loop on the fiducial detectors for the current robot
@@ -691,9 +752,9 @@ StageNode::WorldCallback()
691752
}
692753

693754
if (robotmodel->fiducialmodels.size() > 1)
694-
msg.header.frame_id = mapName(BASE_FIDUCIAL_LINK, r, s, static_cast<Stg::Model*>(robotmodel->positionmodel));
755+
msg.header.frame_id = mapTfName(BASE_FIDUCIAL_LINK, r, s, static_cast<Stg::Model*>(robotmodel->positionmodel));
695756
else
696-
msg.header.frame_id = mapName(BASE_FIDUCIAL_LINK, r, static_cast<Stg::Model*>(robotmodel->positionmodel));
757+
msg.header.frame_id = mapTfName(BASE_FIDUCIAL_LINK, r, static_cast<Stg::Model*>(robotmodel->positionmodel));
697758

698759
msg.header.stamp = sim_time;
699760
robotmodel->fiducial_pubs[s].publish(msg);
@@ -707,19 +768,19 @@ StageNode::WorldCallback()
707768

708769
if (robotmodel->lasermodels.size() > 1)
709770
tf.sendTransform(tf::StampedTransform(txFiducial, sim_time,
710-
mapName(BASE_LINK, r, static_cast<Stg::Model*>(robotmodel->positionmodel)),
711-
mapName(BASE_FIDUCIAL_LINK, r, s, static_cast<Stg::Model*>(robotmodel->positionmodel))));
771+
mapTfName(BASE_LINK, r, static_cast<Stg::Model*>(robotmodel->positionmodel)),
772+
mapTfName(BASE_FIDUCIAL_LINK, r, s, static_cast<Stg::Model*>(robotmodel->positionmodel))));
712773
else
713774
tf.sendTransform(tf::StampedTransform(txFiducial, sim_time,
714-
mapName(BASE_LINK, r, static_cast<Stg::Model*>(robotmodel->positionmodel)),
715-
mapName(BASE_FIDUCIAL_LINK, r, static_cast<Stg::Model*>(robotmodel->positionmodel))));
775+
mapTfName(BASE_LINK, r, static_cast<Stg::Model*>(robotmodel->positionmodel)),
776+
mapTfName(BASE_FIDUCIAL_LINK, r, static_cast<Stg::Model*>(robotmodel->positionmodel))));
716777
}
717778

718779
//the position of the robot
719780
tf.sendTransform(tf::StampedTransform(tf::Transform::getIdentity(),
720781
sim_time,
721-
mapName(BASE_FOOTPRINT, r, static_cast<Stg::Model*>(robotmodel->positionmodel)),
722-
mapName(BASE_LINK, r, static_cast<Stg::Model*>(robotmodel->positionmodel))));
782+
mapTfName(BASE_FOOTPRINT, r, static_cast<Stg::Model*>(robotmodel->positionmodel)),
783+
mapTfName(BASE_LINK, r, static_cast<Stg::Model*>(robotmodel->positionmodel))));
723784

724785
// Get latest odometry data
725786
// Translate into ROS message format and publish
@@ -735,7 +796,7 @@ StageNode::WorldCallback()
735796
//@todo Publish stall on a separate topic when one becomes available
736797
//this->odomMsgs[r].stall = this->positionmodels[r]->Stall();
737798
//
738-
odom_msg.header.frame_id = mapName(ODOM, r, static_cast<Stg::Model*>(robotmodel->positionmodel));
799+
odom_msg.header.frame_id = mapTfName(ODOM, r, static_cast<Stg::Model*>(robotmodel->positionmodel));
739800
odom_msg.header.stamp = sim_time;
740801

741802
robotmodel->odom_pub.publish(odom_msg);
@@ -745,8 +806,8 @@ StageNode::WorldCallback()
745806
tf::quaternionMsgToTF(odom_msg.pose.pose.orientation, odomQ);
746807
tf::Transform txOdom(odomQ, tf::Point(odom_msg.pose.pose.position.x, odom_msg.pose.pose.position.y, 0.0));
747808
tf.sendTransform(tf::StampedTransform(txOdom, sim_time,
748-
mapName(ODOM, r, static_cast<Stg::Model*>(robotmodel->positionmodel)),
749-
mapName(BASE_FOOTPRINT, r, static_cast<Stg::Model*>(robotmodel->positionmodel))));
809+
mapTfName(ODOM, r, static_cast<Stg::Model*>(robotmodel->positionmodel)),
810+
mapTfName(BASE_FOOTPRINT, r, static_cast<Stg::Model*>(robotmodel->positionmodel))));
750811

751812
// Also publish the ground truth pose and velocity
752813
Stg::Pose gpose = robotmodel->positionmodel->GetGlobalPose();
@@ -782,7 +843,7 @@ StageNode::WorldCallback()
782843
ground_truth_msg.twist.twist.linear.z = gvel.z;
783844
ground_truth_msg.twist.twist.angular.z = gvel.a;
784845

785-
ground_truth_msg.header.frame_id = mapName(BASE_POSE_GROUND_TRUTH, r, static_cast<Stg::Model*>(robotmodel->positionmodel));
846+
ground_truth_msg.header.frame_id = mapTfName(BASE_POSE_GROUND_TRUTH, r, static_cast<Stg::Model*>(robotmodel->positionmodel));
786847
ground_truth_msg.header.stamp = sim_time;
787848

788849
robotmodel->ground_truth_pub.publish(ground_truth_msg);
@@ -819,9 +880,9 @@ StageNode::WorldCallback()
819880
}
820881

821882
if (robotmodel->cameramodels.size() > 1)
822-
image_msg.header.frame_id = mapName(CAMERA, r, s, static_cast<Stg::Model*>(robotmodel->positionmodel));
883+
image_msg.header.frame_id = mapTfName(CAMERA, r, s, static_cast<Stg::Model*>(robotmodel->positionmodel));
823884
else
824-
image_msg.header.frame_id = mapName(CAMERA, r,static_cast<Stg::Model*>(robotmodel->positionmodel));
885+
image_msg.header.frame_id = mapTfName(CAMERA, r,static_cast<Stg::Model*>(robotmodel->positionmodel));
825886
image_msg.header.stamp = sim_time;
826887

827888
robotmodel->image_pubs[s].publish(image_msg);
@@ -877,9 +938,9 @@ StageNode::WorldCallback()
877938
}
878939

879940
if (robotmodel->cameramodels.size() > 1)
880-
depth_msg.header.frame_id = mapName(CAMERA, r, s, static_cast<Stg::Model*>(robotmodel->positionmodel));
941+
depth_msg.header.frame_id = mapTfName(CAMERA, r, s, static_cast<Stg::Model*>(robotmodel->positionmodel));
881942
else
882-
depth_msg.header.frame_id = mapName(CAMERA, r, static_cast<Stg::Model*>(robotmodel->positionmodel));
943+
depth_msg.header.frame_id = mapTfName(CAMERA, r, static_cast<Stg::Model*>(robotmodel->positionmodel));
883944
depth_msg.header.stamp = sim_time;
884945
robotmodel->depth_pubs[s].publish(depth_msg);
885946
}
@@ -900,18 +961,18 @@ StageNode::WorldCallback()
900961

901962
if (robotmodel->cameramodels.size() > 1)
902963
tf.sendTransform(tf::StampedTransform(tr, sim_time,
903-
mapName(BASE_LINK, r, static_cast<Stg::Model*>(robotmodel->positionmodel)),
904-
mapName(CAMERA, r, s, static_cast<Stg::Model*>(robotmodel->positionmodel))));
964+
mapTfName(BASE_LINK, r, static_cast<Stg::Model*>(robotmodel->positionmodel)),
965+
mapTfName(CAMERA, r, s, static_cast<Stg::Model*>(robotmodel->positionmodel))));
905966
else
906967
tf.sendTransform(tf::StampedTransform(tr, sim_time,
907-
mapName(BASE_LINK, r, static_cast<Stg::Model*>(robotmodel->positionmodel)),
908-
mapName(CAMERA, r, static_cast<Stg::Model*>(robotmodel->positionmodel))));
968+
mapTfName(BASE_LINK, r, static_cast<Stg::Model*>(robotmodel->positionmodel)),
969+
mapTfName(CAMERA, r, static_cast<Stg::Model*>(robotmodel->positionmodel))));
909970

910971
sensor_msgs::CameraInfo camera_msg;
911972
if (robotmodel->cameramodels.size() > 1)
912-
camera_msg.header.frame_id = mapName(CAMERA, r, s, static_cast<Stg::Model*>(robotmodel->positionmodel));
973+
camera_msg.header.frame_id = mapTfName(CAMERA, r, s, static_cast<Stg::Model*>(robotmodel->positionmodel));
913974
else
914-
camera_msg.header.frame_id = mapName(CAMERA, r, static_cast<Stg::Model*>(robotmodel->positionmodel));
975+
camera_msg.header.frame_id = mapTfName(CAMERA, r, static_cast<Stg::Model*>(robotmodel->positionmodel));
915976
camera_msg.header.stamp = sim_time;
916977
camera_msg.height = cameramodel->getHeight();
917978
camera_msg.width = cameramodel->getWidth();

0 commit comments

Comments
 (0)