Skip to content

Commit

Permalink
fixed launch files
Browse files Browse the repository at this point in the history
  • Loading branch information
ysl208 committed Nov 8, 2017
1 parent 835eda3 commit 247b489
Show file tree
Hide file tree
Showing 4 changed files with 23 additions and 78 deletions.
5 changes: 2 additions & 3 deletions launch/baxter.launch
Original file line number Diff line number Diff line change
Expand Up @@ -16,14 +16,14 @@
<node pkg="rapid_pbd" type="program_execution_node" name="program_executor">
<remap from="l_arm_joint_action" to="/robot/limb/left/follow_joint_trajectory" />
<remap from="r_arm_joint_action" to="/robot/limb/right/follow_joint_trajectory" />
<remap from="l_gripper_action" to="/robot/end_effector/left_gripper/gripper_action" />
<remap from="r_gripper_action" to="/robot/end_effector/right_gripper/gripper_action" />
<remap from="move_group" to="/move_group" />
</node>
<rosparam command="load" file="$(find rapid_pbd)/config/surface_segmentation.yaml" />
<node pkg="rapid_pbd" type="surface_segmentation_node" name="surface_segmentation_node" args="$(arg point_cloud_topic)" />
</group>

<node pkg="tf" type="static_transform_publisher" name="world_to_camera" args="0 0 1 0 0 0 base camera_link 10" />

<!-- Depth cloud -->
<node pkg="nodelet" type="nodelet" name="rapid_pbd_convert_metric" args="standalone depth_image_proc/convert_metric" if="$(arg sim)">
<remap from="image_raw" to="$(arg depth_topic)" />
Expand All @@ -35,5 +35,4 @@
<param unless="$(arg sim)" name="depth" value="$(arg depth_topic)_float" />
<param name="camera_frame_id" value="$(arg camera_frame)" />
</node>
<include file="$(find rapid_pbd)/launch/tf_frames.launch" />
</launch>
91 changes: 19 additions & 72 deletions launch/baxter_moveit.launch
Original file line number Diff line number Diff line change
@@ -1,81 +1,28 @@
<?xml version="1.0"?>
<launch>

<!-- By default we do not overwrite the URDF. Change the following to true to change the default behavior -->
<arg name="load_robot_description" default="false"/>
<!-- By default we DO overwrite the URDF in order to get the grippers. -->
<arg name="load_robot_description" default="true"/>
<!-- Left and right electric gripper params. Set to true to check for collisions for their links -->
<arg name="right_electric_gripper" default="false"/>
<arg name="left_electric_gripper" default="false"/>
<arg name="right_electric_gripper" default="true"/>
<arg name="left_electric_gripper" default="true"/>
<!-- Set the kinematic tips for the left_arm and right_arm move_groups -->
<arg name="left_tip_name" default="left_gripper"/>
<arg name="right_tip_name" default="right_gripper"/>
<include file="$(find rapid_pbd)/launch/baxter_planning_context.launch" />

<!-- GDB Debug Option -->
<arg name="debug" default="false" />
<arg unless="$(arg debug)" name="launch_prefix" value="" />
<arg if="$(arg debug)" name="launch_prefix"
value="gdb -x $(find baxter_moveit_config)/launch/gdb_settings.gdb --ex run --args" />

<!-- Verbose Mode Option -->
<arg name="info" default="$(arg debug)" />
<arg unless="$(arg info)" name="command_args" value="" />
<arg if="$(arg info)" name="command_args" value="--debug" />

<!-- move_group settings -->
<arg name="allow_trajectory_execution" default="true"/>
<arg name="allow_active_sensing" default="false"/>
<arg name="fake_execution" default="false"/>
<arg name="max_safe_path_cost" default="1"/>
<arg name="jiggle_fraction" default="0.05" />
<arg name="publish_monitored_planning_scene" default="true"/>
<arg name="pipeline" default="$(optenv PLANNING_PIPELINE ompl)" />

<!-- Planning Functionality -->
<include ns="move_group" file="$(find baxter_moveit_config)/launch/planning_pipeline.launch">
<arg name="pipeline" value="$(arg pipeline)" />
</include>

<!-- Trajectory Execution Functionality -->
<include ns="move_group" file="$(find baxter_moveit_config)/launch/trajectory_execution.launch" if="$(arg allow_trajectory_execution)">
<arg name="moveit_manage_controllers" value="true" />
<arg name="moveit_controller_manager" value="baxter" unless="$(arg fake_execution)"/>
<arg name="moveit_controller_manager" value="fake" if="$(arg fake_execution)"/>
<include file="$(find baxter_moveit_config)/launch/planning_context.launch">
<arg name="load_robot_description" value="$(arg load_robot_description)"/>
<arg name="left_electric_gripper" value="$(arg left_electric_gripper)"/>
<arg name="right_electric_gripper" value="$(arg right_electric_gripper)"/>
<arg name="left_tip_name" value="$(arg left_tip_name)"/>
<arg name="right_tip_name" value="$(arg right_tip_name)"/>
</include>

<!-- Sensors Functionality -->
<rosparam command="delete" param="move_group/sensors" />
<include ns="move_group" file="$(find baxter_moveit_config)/launch/sensor_manager.launch.xml" if="$(arg allow_active_sensing)">
<arg name="moveit_sensor_manager" value="baxter" />
<arg name="kinect" default="false" />
<arg name="xtion" default="false" />
<arg name="camera_link_pose" default="0.15 0.075 0.5 0.0 0.7854 0.0"/>
<include file="$(find baxter_moveit_config)/launch/move_group.launch">
<arg name="kinect" value="$(arg kinect)" />
<arg name="xtion" value="$(arg xtion)" />
<arg name="camera_link_pose" default="$(arg camera_link_pose)"/>
<arg name="allow_trajectory_execution" value="true"/>
</include>

<!-- Start the actual move_group node/action server -->
<node name="move_group" launch-prefix="$(arg launch_prefix)" pkg="moveit_ros_move_group" type="move_group" respawn="false" output="screen" args="$(arg command_args)">
<!-- Set the display variable, in case OpenGL code is used internally -->
<env name="DISPLAY" value="$(optenv DISPLAY :0)" />

<param name="allow_trajectory_execution" value="$(arg allow_trajectory_execution)"/>
<param name="max_safe_path_cost" value="$(arg max_safe_path_cost)"/>
<param name="jiggle_fraction" value="$(arg jiggle_fraction)" />

<!-- MoveGroup capabilities to load -->
<param name="capabilities" value="move_group/MoveGroupCartesianPathService
move_group/MoveGroupExecuteService
move_group/MoveGroupKinematicsService
move_group/MoveGroupMoveAction
move_group/MoveGroupPickPlaceAction
move_group/MoveGroupPlanService
move_group/MoveGroupQueryPlannersService
move_group/MoveGroupStateValidationService
move_group/MoveGroupGetPlanningSceneService
move_group/ApplyPlanningSceneService
move_group/ClearOctomapService
" />

<!-- Publish the planning scene of the physical robot so that rviz plugin can know actual robot -->
<param name="planning_scene_monitor/publish_planning_scene" value="$(arg publish_monitored_planning_scene)" />
<param name="planning_scene_monitor/publish_geometry_updates" value="$(arg publish_monitored_planning_scene)" />
<param name="planning_scene_monitor/publish_state_updates" value="$(arg publish_monitored_planning_scene)" />
<param name="planning_scene_monitor/publish_transforms_updates" value="$(arg publish_monitored_planning_scene)" />
</node>

</launch>
4 changes: 2 additions & 2 deletions launch/tf_frames.launch
Original file line number Diff line number Diff line change
@@ -1,5 +1,5 @@
<launch>
<include file="$(find rgbd_launch)/launch/kinect_frames.launch" />
<!--include file="$(find rgbd_launch)/launch/kinect_frames.launch" /-->
<node pkg="tf" type="static_transform_publisher" name="world_to_camera" args="0 0 1 0 0 0 base camera_link 100" />
<node pkg="tf" type="static_transform_publisher" name="thermal_extrinsic_calibration_publisher" args="-0.00091294711 -0.040564451 -0.025354201 -0.03274371 -0.00118647 0.00181309 0.99932802 camera_rgb_optical_frame thermal_camera_frame 100" />
<!--node pkg="tf" type="static_transform_publisher" name="thermal_extrinsic_calibration_publisher" args="-0.00091294711 -0.040564451 -0.025354201 -0.03274371 -0.00118647 0.00181309 0.99932802 camera_rgb_optical_frame thermal_camera_frame 100" /-->
</launch>
1 change: 0 additions & 1 deletion src/robot_config.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -204,7 +204,6 @@ void BaxterRobotConfig::default_gripper_poses(
void BaxterRobotConfig::joints_for_group(
const std::string& actuator_group,
std::vector<std::string>* joint_names) const {
joint_names->clear();
if (actuator_group == Action::LEFT_ARM) {
joint_names->push_back("left_e0");
joint_names->push_back("left_e1");
Expand Down

0 comments on commit 247b489

Please sign in to comment.