void displayStates(robot_state::RobotState& start_state,
robot_state::RobotState& goal_state, ros::NodeHandle& node_handle,
robot_model::RobotModelPtr& robot_model)
{
static ros::Publisher start_state_display_publisher = node_handle.advertise<
moveit_msgs::DisplayRobotState>("/move_itomp/display_start_state",
1, true);
static ros::Publisher goal_state_display_publisher = node_handle.advertise<
moveit_msgs::DisplayRobotState>("/move_itomp/display_goal_state", 1,
true);
int num_variables = start_state.getVariableNames().size();
moveit_msgs::DisplayRobotState disp_start_state;
disp_start_state.state.joint_state.header.frame_id =
robot_model->getModelFrame();
disp_start_state.state.joint_state.name = start_state.getVariableNames();
disp_start_state.state.joint_state.position.resize(num_variables);
memcpy(&disp_start_state.state.joint_state.position[0],
start_state.getVariablePositions(), sizeof(double) * num_variables);
disp_start_state.highlight_links.clear();
const std::vector<std::string>& link_model_names =
robot_model->getLinkModelNames();
for (int i = 0; i < link_model_names.size(); ++i)
{
std_msgs::ColorRGBA color;
color.a = 0.5;
color.r = 0.0;
color.g = 1.0;
color.b = 0.5;
moveit_msgs::ObjectColor obj_color;
obj_color.id = link_model_names[i];
obj_color.color = color;
disp_start_state.highlight_links.push_back(obj_color);
}
start_state_display_publisher.publish(disp_start_state);
moveit_msgs::DisplayRobotState disp_goal_state;
disp_goal_state.state.joint_state.header.frame_id =
robot_model->getModelFrame();
disp_goal_state.state.joint_state.name = goal_state.getVariableNames();
disp_goal_state.state.joint_state.position.resize(num_variables);
memcpy(&disp_goal_state.state.joint_state.position[0],
goal_state.getVariablePositions(), sizeof(double) * num_variables);
disp_goal_state.highlight_links.clear();
for (int i = 0; i < link_model_names.size(); ++i)
{
std_msgs::ColorRGBA color;
color.a = 0.5;
color.r = 0.0;
color.g = 0.5;
color.b = 1.0;
moveit_msgs::ObjectColor obj_color;
obj_color.id = link_model_names[i];
obj_color.color = color;
disp_goal_state.highlight_links.push_back(obj_color);
}
goal_state_display_publisher.publish(disp_goal_state);
}
void displayInitialWaypoints(robot_state::RobotState& state,
ros::NodeHandle& node_handle,
robot_model::RobotModelPtr& robot_model,
const std::vector<std::string>& hierarchy,
const std::vector<Eigen::VectorXd>& waypoints)
{
static ros::Publisher vis_marker_array_publisher = node_handle.advertise<visualization_msgs::MarkerArray>("/move_itomp/visualization_marker_array", 10);
visualization_msgs::MarkerArray ma;
std::vector<std::string> link_names = robot_model->getLinkModelNames();
std_msgs::ColorRGBA color;
color.a = 0.5;
color.r = 1.0;
color.g = 1.0;
color.b = 0.0;
//ros::Duration dur(3600.0);
ros::Duration dur(0.25);
for (unsigned int point = 0; point < waypoints.size(); ++point)
{
ma.markers.clear();
setRobotStateFrom(state, hierarchy, waypoints, point);
double time = 0.05;
ros::WallDuration timer(time);
timer.sleep();
std::string ns = "init_" + boost::lexical_cast<std::string>(point);
state.getRobotMarkers(ma, link_names, color, ns, dur);
vis_marker_array_publisher.publish(ma);
}
}
void renderHierarchicalTrajectory(
robot_trajectory::RobotTrajectoryPtr& robot_trajectory,
ros::NodeHandle& node_handle, robot_model::RobotModelPtr& robot_model)
{
static ros::Publisher vis_marker_array_publisher_ = node_handle.advertise<
visualization_msgs::MarkerArray>("itomp_planner/trajectory", 10);
visualization_msgs::MarkerArray ma;
std::vector<std::string> link_names = robot_model->getLinkModelNames();
std_msgs::ColorRGBA color;
std::string ns = "robot";
ros::Duration dur(100.0);
std::map<std::string, std_msgs::ColorRGBA> colorMap;
color.a = 0.3;
color.r = 0.5;
color.g = 1.0;
color.b = 1.0;
colorMap["lower_body"] = color;
color.a = 0.3;
color.r = 0.5;
color.g = 1.0;
color.b = 0.3;
colorMap["torso"] = color;
color.g = 0.3;
color.b = 1.0;
colorMap["head"] = color;
color.r = 0.9;
color.g = 0.5;
color.b = 0.3;
colorMap["left_arm"] = color;
color.r = 0.9;
color.g = 0.3;
color.b = 0.5;
colorMap["right_arm"] = color;
color.r = 1.0;
color.g = 1.0;
color.b = 1.0;
colorMap["object"] = color;
const robot_state::JointModelGroup* joint_model_group;
std::map<std::string, std::vector<std::string> > group_links_map;
group_links_map["lower_body"] = robot_model->getJointModelGroup(
"lower_body")->getLinkModelNames();
group_links_map["torso"] =
robot_model->getJointModelGroup("torso")->getLinkModelNames();
group_links_map["head"] =
robot_model->getJointModelGroup("head")->getLinkModelNames();
group_links_map["left_arm"] =
robot_model->getJointModelGroup("left_arm")->getLinkModelNames();
group_links_map["right_arm"] =
robot_model->getJointModelGroup("right_arm")->getLinkModelNames();
group_links_map["object"].clear();
if (robot_model->hasLinkModel("right_hand_object_link"))
group_links_map["object"].push_back("right_hand_object_link");
int num_waypoints = robot_trajectory->getWayPointCount();
for (int i = 0; i < num_waypoints; ++i)
{
ma.markers.clear();
robot_state::RobotStatePtr state = robot_trajectory->getWayPointPtr(i);
for (std::map<std::string, std::vector<std::string> >::iterator it =
group_links_map.begin(); it != group_links_map.end(); ++it)
{
std::string ns = "robot_" + it->first;
state->getRobotMarkers(ma, group_links_map[it->first],
colorMap[it->first], ns, dur);
}
vis_marker_array_publisher_.publish(ma);
double time = (i == 0 || i == num_waypoints - 1) ? 2.0 : 0.05;
ros::WallDuration timer(time);
timer.sleep();
}
for (int i = 0; i < 10; ++i)
{
ma.markers.clear();
robot_state::RobotStatePtr state = robot_trajectory->getWayPointPtr(
num_waypoints - 1);
for (std::map<std::string, std::vector<std::string> >::iterator it =
group_links_map.begin(); it != group_links_map.end(); ++it)
{
std::string ns = "robot_" + it->first;
state->getRobotMarkers(ma, group_links_map[it->first],
colorMap[it->first], ns, dur);
}
vis_marker_array_publisher_.publish(ma);
double time = 0.05;
ros::WallDuration timer(time);
timer.sleep();
}
}