Hello experimented ROS user,
I have been trying to develop a teleop code similar to: [Turtle_teleop_joy.cpp](http://wiki.ros.org/joy/Tutorials/WritingTeleopNode).
The robot succesfully moves, plans and executes trajectories using the same constructor we initialize in [move_group_interface_tutorial.cpp](http://docs.ros.org/indigo/api/moveit_tutorials/html/doc/pr2_tutorials/planning/src/doc/move_group_interface_tutorial.html), in the RVIZ tutorials/documentation.
However, each time I click on one of the buttons or axes, my program initializes/load the entire MoveGroup action.
Here's the code:
#include
#include
#include
#include
#include
class TeleOpBlanqui
{
public:
TeleOpBlanqui();
private:
void joyCallback(const sensor_msgs::Joy::ConstPtr& msg);
ros::NodeHandle nh_;
ros::Subscriber sub;
int a;
};
TeleOpBlanqui::TeleOpBlanqui():
a(1)
{
sub = nh_.subscribe("joy", 1000, &TeleOpBlanqui::joyCallback, this);
}
void TeleOpBlanqui::joyCallback (const sensor_msgs::Joy::ConstPtr& msg)
{
if(msg->axes[1] == 1){
ros::AsyncSpinner spinner(1);
spinner.start();
moveit::planning_interface::MoveGroup group("R1");
geometry_msgs::Pose target_pose1;
target_pose1.orientation.w = 1.0;
target_pose1.position.x = group.getCurrentPose().pose.position.x + 0.01;
target_pose1.position.y = group.getCurrentPose().pose.position.y;
target_pose1.position.z = group.getCurrentPose().pose.position.z;
group.setPoseTarget(target_pose1);
moveit::planning_interface::MoveGroup::Plan my_plan;
bool success = group.plan(my_plan);
group.move();
bool success2 = group.move();
}
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "teleop_blanqui");
TeleOpBlanqui teleop_blanqui;
ros::spin();
}
And here's a bit of my terminal on the problem I am describing.
[ INFO] [1523987544.250650398]: Ready to take MoveGroup commands for group R1.
[ INFO] [1523987545.013332403]: TrajectoryExecution will use new action capability.
[ INFO] [1523987545.013401146]: Ready to take MoveGroup commands for group R1.
[ INFO] [1523987546.085418516]: TrajectoryExecution will use new action capability.
[ INFO] [1523987546.085479214]: Ready to take MoveGroup commands for group R1.
[ INFO] [1523987547.087559814]: TrajectoryExecution will use new action capability.
[ INFO] [1523987547.087696807]: Ready to take MoveGroup commands for group R1.
[ INFO] [1523987548.905201259]: TrajectoryExecution will use new action capability.
[ INFO] [1523987548.905298749]: Ready to take MoveGroup commands for group R1.
I initialized the variables `int a;` because the program would throw a compilation error if I didn't gave parameters.
Also, I have a larger and more complete code with all of the axes and buttons of the Joystick, however I don't consider relevant uploading the whole thing since it's just the same code, with other increments/decrements in positions.
I want the MoveGroup to load one time and be able to move the robot on real time.
I am open to new suggestions.
I am using Indigo on Ubuntu 14.04
↧