Carollour's profile - activity

Hello everyone,

I am trying to read from a rosbag within my c++ node, and store the value of a message in a variable: This is my rosbag info:

path:        moveToPreGraspPosition.bag
version:     2.0
duration:    0.0s
start:       Nov 03 2015 09:50:28.73 (1446544228.73)
end:         Nov 03 2015 09:50:28.73 (1446544228.73)
size:        21.0 KB
messages:    1
compression: none [1/1 chunks]
types:       control_msgs/FollowJointTrajectoryActionGoal [cff5c1d533bf2f82dd0138d57f4304bb]
topics:      /mbot05/cyton_joint_trajectory_action_controller/follow_joint_trajectory/goal   1 msg     : control_msgs/FollowJointTrajectoryActionGoal

So, I have one message of type control_msgs/FollowJointTrajectoryActionGoal and I want to create a variable of this type in my code and store this info in that variable. I followed some tutorials online and this is what I came up with:

actionlib::SimpleActionClient<control_msgs::FollowJointTrajectoryAction> ac("cyton_joint_trajectory_action_controller/follow_joint_trajectory", true);
ac.waitForServer(); //will wait for infinite time

rosbag::Bag bag;
bag.open("/home/catkin_ws/src/pick_place_demo/moveToPreGraspPosition.bag", rosbag::bagmode::Read);

std::vector<std::string> topics;
topics.push_back(std::string("/mbot05/cyton_joint_trajectory_action_controller/follow_joint_trajectory/goal"));
rosbag::View view(bag, rosbag::TopicQuery(topics));

BagSubscriber<control_msgs::FollowJointTrajectoryActionGoal> teste;  

BOOST_FOREACH(rosbag::MessageInstance const m, view)
{
    control_msgs::FollowJointTrajectoryActionGoal::ConstPtr s = m.instantiate<control_msgs::FollowJointTrajectoryActionGoal>();
    if (s != NULL)
    teste.newMessage(s);
}

bag.close();
ac.sendGoal(teste.goal);

//control_msgs::FollowJointTrajectoryActionGoal sd2;
//ac.sendGoal(sd2.goal);

bool finished_before_timeout = ac.waitForResult(ros::Duration(30.0));
if (finished_before_timeout){
    actionlib::SimpleClientGoalState state = ac.getState();
    ROS_INFO("Action finished: %s",state.toString().c_str());
}else{
    ROS_INFO("Action did not finish before the time out.");
}

Where BagSubscriber is defined like this:

template <class M>
class BagSubscriber : public message_filters::SimpleFilter<M>
{
public:
  void newMessage(const boost::shared_ptr<M const> &msg)
  {
    signalMessage(msg);
  }
};

When I run this, however, I cannot access the field "goal" inside my variable because I have the wrong type, giving me a compiling error:

/home/catkin_ws/src/pick_place_demo/src/PickObject.cpp:108:21: error: ‘class BagSubscriber<control_msgs::FollowJointTrajectoryActionGoal_<std::allocator<void> > >’ has no member named ‘goal’

If run the commented lines instead, it compiles smoothly. Do you know how I can copy the contents of my bag topic to my variable so it's clean? If I try other approaches to make s igual to sd2 I usually get this:

no known conversion for argument 1 from ‘control_msgs::FollowJointTrajectoryActionGoal_<std::allocator<void> >::ConstPtr {aka boost::shared_ptr<const control_msgs::FollowJointTrajectoryActionGoal_<std::allocator<void> > >}’ to ‘const control_msgs::FollowJointTrajectoryActionGoal_<std::allocator<void> >&’

HI,

I have saved a rosbag with the goal topic of an action:

...
messages:    1
compression: none [1/1 chunks]
types:       control_msgs/FollowJointTrajectoryActionGoal [cff5c1d533bf2f82dd0138d57f4304bb]
topics:      /robot_joint_trajectory_action_controller/follow_joint_trajectory/goal   1 msg     : control_msgs/FollowJointTrajectoryActionGoal

What I was trying to do was to copy this 1 message to a variable so I can call the service with this saved goal within my cpp node:

rosbag::Bag bag;
bag.open("test.bag", rosbag::bagmode::Read);

std::vector<std::string> topics;
topics.push_back(std::string("/robot_joint_trajectory_action_controller/follow_joint_trajectory/goal"));

rosbag::View view(bag, rosbag::TopicQuery(topics));

foreach(rosbag::MessageInstance const m, view)
{
    control_msgs::FollowJointTrajectoryActionGoal::ConstPtr s = m.instantiate<control_msgs::FollowJointTrajectoryActionGoal>();
    if (s != NULL){
        goal=s;
    }
}

bag.close();

//ac.sendGoal(goal);

//wait for the action to return
bool finished_before_timeout = ac.waitForResult(ros::Duration(30.0));

if (finished_before_timeout)
{
    actionlib::SimpleClientGoalState state = ac.getState();
    ROS_INFO("Action finished: %s",state.toString().c_str());
}
else{
    ROS_INFO("Action did not finish before the time out.");
}

Of course i cannot do s=goal. How to do this?

Thanks!

Were you able to find a solution for #2?

I have both a catkin_ws and a rosbuild_ws. However, when I use the command roscd I can only go to the one I've sourced last, i.e.:

-When I do source ~/rosbuild_ws/setup.bash, I the packages inside rosbuild are known, hence I can roscd to them.

-When I do ". ~/catkin_ws/devel/setup.bash", the packages inside catkin are known. But I stop being able to roscd into rosbuild.

This also means that rosbuild cannot use catkin packages and vice versa.

What can I do?

This is my workspace

>>Folder1
    >>scr
        >>Gripper.cpp
    >>include
        >>Gripper.h
    >>script
        >>Arm.py

Inside Arm.py , I've got a class ArmClass defined, with some variables and functions. All the code I'm running is inside Gripper.cpp. I need to create a member of ArmClass inside my gripper.cpp. I was reading about this online but I couldn't figure out how to do it and didn't find any information on how to do it in ROS (I only found how to call Cpp classes in python code...)

Thanks

I was wondering if it is possible to instead of planning positions for the last joint of the arm to go to, if it's possible to do control the end-effector (gripper) position instead... I've been trying to do this for a while but can't figure out how to. thanks

Hi,

I’m trying to use move it to move an arm vertically ONLY. The idea is to keep the tip of the end-effector to always keep the x and y-axis position and change the z-axis position in each iteration, keeping its orientation as well. I also want to constrain the movement from start-position to end-position in each iteration to follow this constraints (x and y fixed, z changing only). I don’t care about how much the joints change as long as the gripper (my end-effector) only moves upwards.

I tried to do it as presented bellow, but i don’t see any constraints being followed? What am I doing wrong?

int main(int argc, char **argv){

    ros::init(argc, argv, "move_group_interface_tutorial");
    ros::NodeHandle node_handle;
    ros::AsyncSpinner spinner(1);
    spinner.start();


    /* This sleep is ONLY to allow Rviz to come up */
    sleep(20.0);

    moveit::planning_interface::MoveGroup group_r("right_arm");

    robot_state::RobotState start_state(*group_r.getCurrentState());
    geometry_msgs::Pose start_pose;
    start_pose.orientation.x = group_r.getCurrentPose().pose.orientation.x;
    start_pose.orientation.y = group_r.getCurrentPose().pose.orientation.y;
    start_pose.orientation.z = group_r.getCurrentPose().pose.orientation.z;
    start_pose.orientation.w = group_r.getCurrentPose().pose.orientation.w;
    start_pose.position.x = group_r.getCurrentPose().pose.position.x;
    start_pose.position.y = group_r.getCurrentPose().pose.position.y;
    start_pose.position.z = group_r.getCurrentPose().pose.position.z;

    //const robot_state::JointModelGroup *joint_model_group = start_state.getJointModelGroup(group_r.getName());
    //start_state.setFromIK(joint_model_group, start_pose);
    group_r.setStartState(start_state);

    moveit_msgs::OrientationConstraint ocm;

    ocm.link_name = "r_wrist_roll_link";
    ocm.header.frame_id = "base_link";
    ocm.orientation.w = 0.0;
    ocm.absolute_x_axis_tolerance = 0.0;
    ocm.absolute_y_axis_tolerance = 0.0;
    ocm.absolute_z_axis_tolerance = 0.1;
    ocm.weight = 1.0;
    moveit_msgs::Constraints test_constraints;
    test_constraints.orientation_constraints.push_back(ocm);
    group_r.setPathConstraints(test_constraints);



    geometry_msgs::Pose next_pose = start_pose;
    while(1){
        std::vector<geometry_msgs::Pose> waypoints;
        next_pose.position.z -= 0.03;
        waypoints.push_back(next_pose);  // up and out

        moveit_msgs::RobotTrajectory trajectory;
        double fraction = group_r.computeCartesianPath(waypoints, 0.005, 0.0, trajectory);

        /* Sleep to give Rviz time to visualize the plan. */
        sleep(5.0);
    }


}

thank you, that was it.

Thank you, that helped!

I have set a loop rate in the following way:

ros::Rate r(100)
while(ros::Ok()){
    ros::SpinOnce();
    function();
    r.sleep();
}

Is there anyway to verify if I am accomplish this rate or if the function is taking longer than that? Thank you.

I have three different but related questions

1st: Is it possible to set the value of a variable using the launch file? I read about it online but I couldn't understand how to use it.

2nd: I couldn't find anything on how to use the launch file to set the value of a variable in a message. for instance if I have:

mymessagevector.msg:

mymessage myvector[2]

mymessage.msg:

int32 value
string ID

Is there a way to set the myvector[0].ID="ID1" and myvector[1].ID="ID2" and always publish that without having to define it everytime I sent the message to a topic?

3rd: If I don't know how many variables I have and I want to publish each variable in a different topic like /commontopic/var1 ; /commontopic/var2 ; /commontopic/varN , is there a way to achieve this using the terminal? Thank you in advance