Skip to content

Commit 49ab031

Browse files
author
kut
committed
extract some rosparams to abstract away the target robot
1 parent a44f9c2 commit 49ab031

File tree

8 files changed

+139
-135
lines changed

8 files changed

+139
-135
lines changed

‎config/prbt.yaml‎

Lines changed: 4 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,4 @@
1+
robot: prbt
2+
group_manip: manipulator
3+
max_planning_attempts: 4
4+
robot_settle_time: 0.4

‎config/ur5e.yaml‎

Lines changed: 4 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,4 @@
1+
robot: ur5e
2+
group_manip: manipulator
3+
max_planning_attempts: 3
4+
robot_settle_time: 0.4

‎include/ur_manipulation/moveit_custom_api.hpp‎

Lines changed: 4 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -24,11 +24,9 @@ class MoveitCustomApi
2424
{
2525
public:
2626
MoveitCustomApi();
27-
MoveitCustomApi(int max_trials, std::string user_prompts);
27+
MoveitCustomApi(std::string user_prompts);
2828
~MoveitCustomApi();
2929

30-
const std::string GROUP_MANIP = "manipulator";
31-
const std::string GROUP_GRIPP = "endeffector";
3230
const std::string COMMAND_ADD = "add";
3331
const std::string COMMAND_REMOVE = "remove";
3432

@@ -64,6 +62,9 @@ class MoveitCustomApi
6462
moveit_visual_tools::MoveItVisualTools *visual_tools;
6563
Eigen::Isometry3d text_pose = Eigen::Isometry3d::Identity();
6664
const int IO_SERVICE_FUN_LEVEL_ = 1; // Not exactly sure what this is, but 1 seems to work. If it fails, try 2.
65+
std::string robot_name_;
66+
std::string group_manip_;
67+
double robot_settle_time_;
6768

6869
const double BASE_OFFSET_FROM_BACK_WALL_ = 0.28; //28cm
6970
const double BASE_OFFSET_FROM_LEFT_WALL_ = 0.46; //46cm

‎launch/endurance_demo.launch‎

Lines changed: 3 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -2,7 +2,8 @@
22
<launch>
33

44
<arg name="prompts" default="true" />
5-
<arg name="max_trials" default="5" />
6-
<node name="endurance_demo" pkg="ur_manipulation" type="endurance_demo" respawn="false" output="screen" args="$(arg prompts) $(arg max_trials)"/>
5+
<arg name="robot" default="ur5e" />
6+
<rosparam file="$(find ur_manipulation)/config/$(arg robot).yaml"/>
7+
<node name="endurance_demo" pkg="ur_manipulation" type="endurance_demo" respawn="false" output="screen" args="$(arg prompts)"/>
78
<node name="tool_pose_publisher" pkg ="ur_manipulation" type="tool_pose_publisher.py" output="screen"/>
89
</launch>

‎launch/seher_demo.launch‎

Lines changed: 3 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -2,7 +2,8 @@
22
<launch>
33

44
<arg name="prompts" default="true" />
5-
<arg name="max_trials" default="5" />
6-
<node name="seher_demo" pkg="ur_manipulation" type="seher_demo" respawn="false" output="screen" args="$(arg prompts) $(arg max_trials)"/>
5+
<arg name="robot" default="ur5e" />
6+
<rosparam file="$(find ur_manipulation)/config/$(arg robot).yaml"/>
7+
<node name="seher_demo" pkg="ur_manipulation" type="seher_demo" respawn="false" output="screen" args="$(arg prompts)"/>
78
<node name="tool_pose_publisher" pkg ="ur_manipulation" type="tool_pose_publisher.py" output="screen"/>
89
</launch>

‎src/endurance_demo.cpp‎

Lines changed: 37 additions & 37 deletions
Original file line numberDiff line numberDiff line change
@@ -23,50 +23,50 @@ int main(int argc, char **argv)
2323

2424
//-------------Pick and place program-------------------
2525

26-
// ros::Publisher planning_scene_diff_publisher = nh.advertise<moveit_msgs::PlanningScene>("planning_scene", 1);
27-
// ros::Publisher pub_seq = nh.advertise<std_msgs::Header>("/ur_manipulation/sequence",1);
28-
// ros::Publisher pub_fail = nh.advertise<std_msgs::Header>("/ur_manipulation/failure_counter",1);
26+
ros::Publisher planning_scene_diff_publisher = nh.advertise<moveit_msgs::PlanningScene>("planning_scene", 1);
27+
ros::Publisher pub_seq = nh.advertise<std_msgs::Header>("/ur_manipulation/sequence",1);
28+
ros::Publisher pub_fail = nh.advertise<std_msgs::Header>("/ur_manipulation/failure_counter",1);
2929

30-
// MoveitCustomApi seher_obj(atoi(argv[2]),argv[1]);
31-
// seher_obj.initialiseMoveit(nh);
32-
// seher_obj.printBasicInfo();
33-
// ROS_INFO("---------------------------");
34-
// seher_obj.addCollissionObjects();
35-
// ROS_INFO("Moving to home pose");
36-
// seher_obj.moveToNamedTarget("home");
37-
// unsigned int seq = 0;
30+
MoveitCustomApi seher_obj(argv[1]);
31+
seher_obj.initialiseMoveit(nh);
32+
seher_obj.printBasicInfo();
33+
ROS_INFO("---------------------------");
34+
seher_obj.addCollissionObjects();
35+
ROS_INFO("Moving to home pose");
36+
seher_obj.moveToNamedTarget("home");
37+
unsigned int seq = 0;
3838

39-
// ROS_INFO("Starting PnP");
40-
// ROS_INFO("---------------------------");
39+
ROS_INFO("Starting PnP");
40+
ROS_INFO("---------------------------");
4141

42-
// geometry_msgs::Pose target_pose1;
42+
geometry_msgs::Pose target_pose1;
4343

44-
// target_pose1.position.x = 0.3;
45-
// target_pose1.position.y = 0.4;
46-
// target_pose1.position.z = 0.012;
47-
// geometry_msgs::Quaternion quat_msg;
48-
// tf::quaternionTFToMsg(tf::createQuaternionFromRPY(angles::from_degrees(180),angles::from_degrees(0),angles::from_degrees(0)),quat_msg);
49-
// target_pose1.orientation = quat_msg;
44+
target_pose1.position.x = 0.3;
45+
target_pose1.position.y = 0.4;
46+
target_pose1.position.z = 0.012;
47+
geometry_msgs::Quaternion quat_msg;
48+
tf::quaternionTFToMsg(tf::createQuaternionFromRPY(angles::from_degrees(180),angles::from_degrees(0),angles::from_degrees(0)),quat_msg);
49+
target_pose1.orientation = quat_msg;
5050

51-
// geometry_msgs::Pose target_pose2 = target_pose1;
52-
// target_pose1.position.x = 0.05;
51+
geometry_msgs::Pose target_pose2 = target_pose1;
52+
target_pose1.position.x = 0.05;
5353

5454

55-
// bool switcher=false;
56-
// while(ros::ok())
57-
// {
58-
// ROS_INFO_STREAM("----------------------SEQ " << seq << "-------------------------------------");
59-
// seher_obj.pickAtPoseFromHeight((switcher)?target_pose1:target_pose2, 0.03, nh, false);
60-
// seher_obj.placeAtPoseFromHeight((switcher)?target_pose2:target_pose1, 0.03, nh, false);
61-
// switcher = !switcher;
62-
// std_msgs::Header msg;
63-
// msg.stamp = ros::Time::now();
64-
// msg.seq = seq;
65-
// pub_seq.publish(msg);
66-
// msg.seq =seher_obj.failure_counter_;
67-
// pub_fail.publish(msg);
68-
// ROS_INFO_STREAM("----------------------SEQ " << seq++ << "-------------------------------------");
69-
// }
55+
bool switcher=false;
56+
while(ros::ok())
57+
{
58+
ROS_INFO_STREAM("----------------------SEQ " << seq << "-------------------------------------");
59+
seher_obj.pickAtPoseFromHeight((switcher)?target_pose1:target_pose2, 0.03, nh, false);
60+
seher_obj.placeAtPoseFromHeight((switcher)?target_pose2:target_pose1, 0.03, nh, false);
61+
switcher = !switcher;
62+
std_msgs::Header msg;
63+
msg.stamp = ros::Time::now();
64+
msg.seq = seq;
65+
pub_seq.publish(msg);
66+
msg.seq =seher_obj.failure_counter_;
67+
pub_fail.publish(msg);
68+
ROS_INFO_STREAM("----------------------SEQ " << seq++ << "-------------------------------------");
69+
}
7070

7171
//-------------Pick and place program-------------------
7272

‎src/moveit_custom_api.cpp‎

Lines changed: 23 additions & 29 deletions
Original file line numberDiff line numberDiff line change
@@ -23,38 +23,27 @@ void getRPYFromQuaternionMSG(geometry_msgs::Quaternion orientation, double& roll
2323

2424

2525
MoveitCustomApi::MoveitCustomApi() :
26-
max_trials(5),
2726
user_prompts(true)
2827
{
2928

3029
}
3130

32-
MoveitCustomApi::MoveitCustomApi(int max_trials, std::string user_prompts)
31+
MoveitCustomApi::MoveitCustomApi(std::string user_prompts)
3332
{
34-
this->max_trials = max_trials;
3533
this->user_prompts = (user_prompts=="True"|| user_prompts=="true")? true : false;
36-
ROS_INFO_STREAM("User prompts : " << this->user_prompts << " | Max planning trials : " << max_trials);
3734
}
3835

3936
MoveitCustomApi::~MoveitCustomApi() {}
4037

4138
void MoveitCustomApi::printBasicInfo()
4239
{
43-
// Getting Basic Information
44-
// ^^^^^^^^^^^^^^^^^^^^^^^^^
45-
//
46-
// We can print the name of the reference frame for this robot.
47-
ROS_INFO_NAMED("tutorial", "Planning frame: %s", move_group->getPlanningFrame().c_str());
48-
49-
// We can also print the name of the end-effector link for this group.
50-
ROS_INFO_NAMED("tutorial", "End effector link: %s", move_group->getEndEffectorLink().c_str());
51-
52-
// We can get a list of all the groups in the robot:
53-
ROS_INFO_NAMED("tutorial", "Available Planning Groups:");
54-
std::copy(move_group->getJointModelGroupNames().begin(), move_group->getJointModelGroupNames().end(),
55-
std::ostream_iterator<std::string>(std::cout, ", "));
56-
std::cout << std::endl;
57-
40+
ROS_INFO("------------------------------------------------------");
41+
ROS_INFO_STREAM("Planning frame: " << move_group->getPlanningFrame().c_str());
42+
ROS_INFO_STREAM("End effector link: " << move_group->getEndEffectorLink().c_str());
43+
ROS_INFO_STREAM("User prompts : " << (this->user_prompts?"True":"False"));
44+
ROS_INFO_STREAM("Max trials : " << this->max_trials);
45+
ROS_INFO_STREAM("Robot settle time : " << this->robot_settle_time_);
46+
ROS_INFO("------------------------------------------------------");
5847
}
5948

6049
bool MoveitCustomApi::comparePoses(geometry_msgs::Pose pose1, geometry_msgs::Pose pose2, double delta_posistion, double delta_orientation)
@@ -94,7 +83,7 @@ void MoveitCustomApi::executeCartesianTrajForWaypoints(std::vector<geometry_msgs
9483
int iter=0;
9584
for(auto x: trajectory.joint_trajectory.points)
9685
{
97-
ROS_INFO_STREAM(iter++ << " point_time_from_start: " << x.time_from_start);
86+
// ROS_INFO_STREAM(iter++ << " point_time_from_start: " << x.time_from_start);
9887
times_from_start.push_back(x.time_from_start);
9988
}
10089

@@ -173,7 +162,7 @@ void MoveitCustomApi::adjustTrajectoryToFixTimeSequencing(moveit_msgs::RobotTraj
173162
for(int i=0; i< times_from_start.size(); i++)
174163
{
175164
trajectory.joint_trajectory.points[i].time_from_start = times_from_start[i];
176-
ROS_INFO_STREAM("Recomputed time point " << i << " : " << trajectory.joint_trajectory.points[i].time_from_start );
165+
// ROS_INFO_STREAM("Recomputed time point " << i << " : " << trajectory.joint_trajectory.points[i].time_from_start );
177166
}
178167
}
179168

@@ -418,7 +407,7 @@ bool MoveitCustomApi::gripperOpen(ros::NodeHandle nh)
418407
if(client.call(io_msg))
419408
{
420409
ROS_INFO_STREAM("Open gripper initialise : " << ((io_msg.response.success==0)?"Failed":"Succeeded") );
421-
sleepSafeFor(0.5);
410+
sleepSafeFor(robot_settle_time_);
422411
io_msg.request.state = 0;
423412
if(client.call(io_msg))
424413
{
@@ -449,7 +438,7 @@ bool MoveitCustomApi::gripperClose(ros::NodeHandle nh)
449438
if(client.call(io_msg))
450439
{
451440
ROS_INFO_STREAM("Close gripper initialise : " << ((io_msg.response.success==0)?"Failed":"Succeeded") );
452-
sleepSafeFor(0.5);
441+
sleepSafeFor(robot_settle_time_);
453442
io_msg.request.state = 0;
454443
if(client.call(io_msg))
455444
{
@@ -472,8 +461,13 @@ bool MoveitCustomApi::gripperClose(ros::NodeHandle nh)
472461
void MoveitCustomApi::initialiseMoveit(ros::NodeHandle nh)
473462
{
474463
namespace rvt = rviz_visual_tools;
475-
move_group = new moveit::planning_interface::MoveGroupInterface(GROUP_MANIP);
476-
joint_model_group = move_group->getCurrentState()->getJointModelGroup(GROUP_MANIP);
464+
nh.param<std::string>("robot",robot_name_,"robot");
465+
nh.param<std::string>("group_manip",group_manip_,"manip");
466+
nh.param<int>("max_planning_attempts",max_trials,3);
467+
nh.param<double>("robot_settle_time",robot_settle_time_,0.5);
468+
469+
move_group = new moveit::planning_interface::MoveGroupInterface(group_manip_);
470+
joint_model_group = move_group->getCurrentState()->getJointModelGroup(group_manip_);
477471

478472
visual_tools = new moveit_visual_tools::MoveItVisualTools(move_group->getPlanningFrame().c_str());
479473
visual_tools->deleteAllMarkers();
@@ -560,14 +554,14 @@ void MoveitCustomApi::pickAtPoseFromHeight(geometry_msgs::Pose target_pose, doub
560554
executeCartesianTrajtoPose(target_pose,"Pre Pick Pose");
561555
}
562556
ROS_INFO("---------------------------");
563-
sleepSafeFor(0.5);
557+
sleepSafeFor(robot_settle_time_);
564558

565559
// Go down to reach and grasp the object
566560
target_pose.position.z-=height;
567561
executeCartesianTrajtoPose(target_pose,"Pick Pose");
568562
// moveGroupExecutePlan(getCartesianPathPlanToPose(target_pose, "Pick Pose"));
569563
if (do_gripper) gripperClose(nh);
570-
sleepSafeFor(0.5);
564+
sleepSafeFor(robot_settle_time_);
571565
// addOrRemoveTestPieceCollissionObjectWRTRobot(COMMAND_ADD);
572566
ROS_INFO("---------------------------");
573567

@@ -591,14 +585,14 @@ void MoveitCustomApi::placeAtPoseFromHeight(geometry_msgs::Pose target_pose, dou
591585
// moveGroupExecutePlan(getCartesianPathPlanToPose(target_pose, "Pre Place Pose"));
592586
executeCartesianTrajtoPose(target_pose,"Pre Place Pose");
593587
ROS_INFO("---------------------------");
594-
sleepSafeFor(0.5);
588+
sleepSafeFor(robot_settle_time_);
595589
// Go down and place the object
596590
target_pose.position.z-=height;
597591
// moveGroupExecutePlan(getCartesianPathPlanToPose(target_pose, "Place Pose"));
598592
executeCartesianTrajtoPose(target_pose,"Place Pose");
599593
ROS_INFO("---------------------------");
600594
if (do_gripper) gripperOpen(nh);
601-
sleepSafeFor(0.5);
595+
sleepSafeFor(robot_settle_time_);
602596
// addOrRemoveTestPieceCollissionObjectWRTRobot(COMMAND_REMOVE);
603597

604598
// Go back up

0 commit comments

Comments
 (0)