Skip to content

[WIP] obstacle distance skip links #112

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
fmessmer wants to merge 4 commits into
base: indigo_dev
Choose a base branch
from
Open

[WIP] obstacle distance skip links #112

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
4 commits
Select commit Hold shift + click to select a range
cec13ac
add example launch
fmessmer Apr 12, 2017
a5a6eb7
skip distance computation for dummy links
fmessmer Apr 12, 2017
290ecc9
improve distance visualization
fmessmer Apr 18, 2017
adc011c
remove registration - topic and service computes all distances
fmessmer Apr 18, 2017
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
37 changes: 37 additions & 0 deletions cob_obstacle_distance_moveit/config/example_skip_links.yaml
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,37 @@
skip_links:
- base_footprint
- fl_caster_rotation_link
- fl_caster_r_wheel_link
- b_caster_rotation_link
- b_caster_r_wheel_link
- fr_caster_rotation_link
- fr_caster_r_wheel_link
- base_laser_front_link
- base_laser_left_link
- base_laser_right_link
- torso_base_link
- torso_1_link
- torso_2_link
- torso_center_link
- torso_cam3d_left_link
- torso_cam3d_left_frame
- torso_cam3d_right_link
- torso_cam3d_right_frame
- torso_cam3d_down_link
- head_base_link
- head_1_link
- head_2_link
- head_center_link
- head_cam_link
- head_cam_frame
- sensorring_base_link
- sensorring_link
- sensorring_cam3d_front_link
- sensorring_cam3d_back_link
- sensorring_cam3d_back_frame
- gripper_right_base_link
- gripper_right_grasp_link
- gripper_right_camera_link
- gripper_left_base_link
- gripper_left_grasp_link
- gripper_left_camera_link
28 changes: 11 additions & 17 deletions cob_obstacle_distance_moveit/include/cob_obstacle_distance_moveit/obstacle_distance_moveit.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -26,39 +26,33 @@ class ObstacleDistanceMoveit
ObstacleDistanceMoveit();

private:
ros::NodeHandle nh_;
ros::NodeHandle nh_;
float MAXIMAL_MINIMAL_DISTANCE;

collision_detection::AllowedCollisionMatrix acm_;
planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor_;
void updatedScene(planning_scene_monitor::PlanningSceneMonitor::SceneUpdateType type);

ros::Timer planning_scene_timer_;
ros::Publisher monitored_scene_pub_;
ros::ServiceServer monitored_scene_server_;
bool planningSceneCallback(moveit_msgs::GetPlanningScene::Request &req, moveit_msgs::GetPlanningScene::Response &res);
void planningSceneTimerCallback(const ros::TimerEvent& event);
bool planningSceneCallback(moveit_msgs::GetPlanningScene::Request &req, moveit_msgs::GetPlanningScene::Response &res);

std::vector< std::string > skip_links_;
std::map<std::string, boost::shared_ptr<fcl::CollisionObject> > robot_links_;
std::map<std::string, boost::shared_ptr<fcl::CollisionObject> > collision_objects_;
std::set< std::string > registered_links_;
boost::mutex registered_links_mutex_;

ros::ServiceServer calculate_obstacle_distance_;
bool calculateDistanceServiceCallback(cob_control_msgs::GetObstacleDistance::Request &req,
cob_control_msgs::GetObstacleDistance::Response &res);

ros::Publisher distance_pub_;
ros::ServiceServer register_server_, unregister_server_;
bool registerCallback(cob_srvs::SetString::Request &req, cob_srvs::SetString::Response &res);
bool unregisterCallback(cob_srvs::SetString::Request &req, cob_srvs::SetString::Response &res);

ros::Timer distance_timer_;
ros::Publisher distance_pub_;
ros::ServiceServer distance_server_;
void calculateDistanceTimerCallback(const ros::TimerEvent& event);
bool calculateDistanceServiceCallback(cob_control_msgs::GetObstacleDistance::Request &req,
cob_control_msgs::GetObstacleDistance::Response &res);

cob_control_msgs::ObstacleDistance getDistanceInfo(const boost::shared_ptr<fcl::CollisionObject> object_a,
const boost::shared_ptr<fcl::CollisionObject> object_b);

collision_detection::AllowedCollisionMatrix acm_;
cob_control_msgs::ObstacleDistances getObstacleDistances();
cob_control_msgs::ObstacleDistance getObstacleDistance(const boost::shared_ptr<fcl::CollisionObject> object_a,
const boost::shared_ptr<fcl::CollisionObject> object_b);
};

#endif // COB_OBSTACLE_DISTANCE_MOVEIT__OBSTACLE_DISTANCE_H
34 changes: 34 additions & 0 deletions cob_obstacle_distance_moveit/launch/example.launch
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,34 @@
<?xml version="1.0"?>
<launch>
<arg name="robot" default="$(env ROBOT)"/>

<!-- upload robot description -->
<include file="$(find cob_hardware_config)/upload_robot.launch">
<arg name="robot" value="$(arg robot)"/>
</include>
<!-- upload semantic description -->
<include file="$(find cob_moveit_config)/launch/upload_config.launch">
<arg name="robot" value="$(arg robot)"/>
<arg name="load_semantic_description" value="true"/>
<arg name="load_planning_context" value="false"/>
</include>

<!-- fake joint_states -->
<node pkg="joint_state_publisher" type="joint_state_publisher" name="joint_state_publisher" output="screen">
<remap from="robot_description" to="/robot_description"/>
<remap from="joint_states" to="/joint_states"/>
<param name="use_gui" value="true"/>
</node>
<node pkg="robot_state_publisher" type="robot_state_publisher" name="robot_state_publisher" output="screen"/>

<!-- obstacle distance moveit node -->
<include file="$(find cob_obstacle_distance_moveit)/launch/obstacle_distance.launch">
<arg name="robot" value="$(arg robot)"/>
<arg name="visualize_distances" value="true"/>
<arg name="skip_links" value="true"/>
</include>

<!-- rviz visualization -->
<node name="rviz" pkg="rviz" type="rviz" args="-d $(find cob_obstacle_distance_moveit)/launch/rviz_config.rviz" />

</launch>
8 changes: 7 additions & 1 deletion cob_obstacle_distance_moveit/launch/obstacle_distance.launch
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,9 +1,11 @@
<?xml version="1.0"?>
<launch>
<arg name="robot" default="$(env ROBOT)"/>
<arg name="robot" default="cob4-5"/>
<arg name="pkg_moveit_config" default="$(find cob_moveit_config)"/>
<arg name="config_path" default="$(arg pkg_moveit_config)/robots/$(arg robot)/moveit/config"/>

<arg name="visualize_distances" default="false"/>
<arg name="skip_links" default="true"/>
<arg name="use_sensors" default="false"/>
<arg name="node_name" value="obstacle_distance_moveit"/>

Expand All @@ -15,5 +17,9 @@

<node pkg="cob_obstacle_distance_moveit" type="obstacle_distance_node" name="$(arg node_name)" output="screen">
<!--remap from="/obstacle_distances" to="/obstacle_distances"/--> <!--only needed for the cob4 specific configuration-->
<rosparam if="$(arg skip_links)" command="load" file="$(find cob_obstacle_distance_moveit)/config/example_skip_links.yaml"/>
</node>

<node if="$(arg visualize_distances)" pkg="cob_obstacle_distance_moveit" type="visualize_obstacle_distance_node" name="visualize_obstacle_distance_node" output="screen"/>

</launch>
Loading