Robotics StackExchange | Archived questions

Control of multiple robot models inside Rviz

I have the URDF file of my robot model. I am able to visualize it inside ROS using the following command:

roslaunch urdf_tutorial display.launch model:=my_robot.urdf

Please note that the above command was used when only one robot, i.e. my_robot is present. Inside the RViz, I can see my_robot after checking RobotModel and I can also see tf frames after adding TF inside the RViz.

Now, In order to move my_robot, I am providing joint angles by writing a joint state publisher in the following way:

#include <ros/ros.h>
#include <sensor_msgs/JointState.h>
#define TOTAL_JOINTS 8

int main(int argc, char** argv)
{
  ros::init(argc, argv, "my_robot_controller");
  ros::NodeHandle n;
  ros::Publisher joint_state_pub = n.advertise<sensor_msgs::JointState>("joint_states", 1);

  sensor_msgs::JointState joint_state;
  joint_state.position.resize(TOTAL_JOINTS);

  // the following joint names are taken from my_robot.urdf file
  joint_state.name = {
      "joint_1", "joint_2", "joint_3", "joint_4",
      "joint_5", "joint_6", "joint_7", "joint_8"};

  joint_state.header.frame_id = "my_robot_link_0";
  while (ros::ok())
  {
      // update the timestamp
      joint_state.header.stamp = ros::Time::now();

      for (int i = 0; i < TOTAL_JOINTS; i++)
        // here is some calculations for joint values
        joint_state.position[i] = joint_value * M_PI / 180.0f;

      joint_state_pub.publish(joint_state);
      ros::spinOnce();
  }
  return 0;
}

The above code works. However, I do have another robot, let say another_robot. This is a commercial robot and I have installed its SDK. Hence whenever I execute this SDK, I can see this robot inside RVIZ.

I want to know how to control and visualize multiple robots inside RViz?

Just for quick understanding, below is the code of display.launch which is inside urdf_tutorial package:

<launch>
  <arg name="model" default="$(find urdf_tutorial)/urdf/01-myfirst.urdf"/>
  <arg name="gui" default="true" />
  <arg name="rvizconfig" default="$(find urdf_tutorial)/rviz/urdf.rviz" />

  <param name="robot_description" command="$(find xacro)/xacro --inorder $(arg model)" />
  <param name="use_gui" value="$(arg gui)"/>

  <node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" />
  <node name="robot_state_publisher" pkg="robot_state_publisher" type="state_publisher" />
  <node name="rviz" pkg="rviz" type="rviz" args="-d $(arg rvizconfig)" required="true" />
</launch>

I want to know how to control and visualize multiple robots inside RViz?

There are the following constraints in this integration task:

Following are the functionalities I am looking for my_robot:

  1. URDF visualization inside RViz
  2. Real-time motions of URDF structure by providing all joint values
  3. TF frames for all links
  4. 3D position of all links with respect to my_robot
  5. 3D position of all links with respect to another_robot

I have the transformation matrix of TF base frame of my_robot with respect to TF base frame of another_robot. So I think If I can attach TF to my_robot, point # 3, 4 and 5 should be easy! Any clue for point #1, 2 and 3, please?

Asked by ravijoshi on 2019-02-22 08:18:47 UTC

Comments

As an observation: I would call "publishing JointState msgs" and viewing the resulting motion in RViz not controlling, but animating a robot model. Control is typically done using msgs other than JointState, as the latter are used to convey state, not command it.

Asked by gvdhoorn on 2019-02-22 11:04:52 UTC

@gvdhoorn: Thank you very much. I have added the exact functionalities, I am looking to incorporate in my_robot. Can you please have a look again? Thank again for the help!

Asked by ravijoshi on 2019-02-22 21:00:12 UTC

Hey, so were you able to find a solution. Im attempting to do the same thing but I cant get the bots to visulaize the map simultaneously. If yes then could please elaborate what all you did and also upload the files on your workspace. Thankyou.

Asked by sparsh on 2020-04-23 09:05:26 UTC

@sparsh please only post answers that are actually answers, this is a Q&A site, not a discussion forum. I've re-posted your answer as a question comment.

Asked by jarvisschultz on 2020-04-23 09:33:38 UTC

Did you find a solution to this problem?

Asked by Andromeda on 2021-07-16 09:33:56 UTC

Answers