sravan/RRRobot
1
0
Fork 0
mirror of https://github.com/EECS-467-W20-RRRobot-Project/RRRobot.git synced 2025-08-26 01:32:44 +00:00
Code Issues Packages Projects Releases Wiki Activity

Added a joint position publisher. It currently publishes all joint angles for the robotic arm, but could be adapted to a different model

Browse Source
This commit is contained in:
Derek Witcpalek
2020-04-03 20:57:47 -04:00
parent 4e30756b75
commit a881191bc6
5 changed files with 52 additions and 18 deletions
Download Patch File Download Diff File Expand all files Collapse all files

18
rrrobot_src/src/simulation_drivers/model_joints.world Normal file
View File

@@ -0,0 +1,18 @@
<?xml version="1.0"?>
<sdf version="1.6">
<world name="default">
<!-- Ground Plane -->
<include>
<uri>model://ground_plane</uri>
</include>
<include>
<uri>model://sun</uri>
</include>
<include>
<uri>model://fanuc_robotic_arm</uri>
</include>
</world>
</sdf>

18
rrrobot_src/src/simulation_drivers/src/simulation_env/CMakeLists.txt
View File

@@ -11,6 +11,7 @@ find_package(catkin REQUIRED COMPONENTS
roscpp roscpp
std_msgs std_msgs
gazebo_ros gazebo_ros
message_generation
) )
find_package(gazebo REQUIRED) find_package(gazebo REQUIRED)
@@ -48,11 +49,10 @@ find_package(gazebo REQUIRED)
## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
## Generate messages in the 'msg' folder ## Generate messages in the 'msg' folder
# add_message_files( add_message_files(
# FILES FILES
# Message1.msg arm_angles.msg
# Message2.msg )
# )
## Generate services in the 'srv' folder ## Generate services in the 'srv' folder
# add_service_files( # add_service_files(
@@ -69,10 +69,10 @@ find_package(gazebo REQUIRED)
# ) # )
## Generate added messages and services with any dependencies listed here ## Generate added messages and services with any dependencies listed here
# generate_messages( generate_messages(
# DEPENDENCIES DEPENDENCIES
# std_msgs std_msgs
# ) )
################################################ ################################################
## Declare ROS dynamic reconfigure parameters ## ## Declare ROS dynamic reconfigure parameters ##

6
rrrobot_src/src/simulation_drivers/src/simulation_env/msg/arm_angles.msg Normal file
View File

@@ -0,0 +1,6 @@
float32 shoulder_pivot_angle
float32 shoulder_joint_angle
float32 elbow_joint_angle
float32 wrist_pivot_angle
float32 wrist_joint_angle
float32 end_effector_pivot_angle

2
rrrobot_src/src/simulation_drivers/src/simulation_env/package.xml
View File

@@ -52,12 +52,14 @@
<build_depend>roscpp</build_depend> <build_depend>roscpp</build_depend>
<build_depend>std_msgs</build_depend> <build_depend>std_msgs</build_depend>
<build_depend>gazebo_ros</build_depend> <build_depend>gazebo_ros</build_depend>
<build_depend>message_generation</build_depend>
<build_export_depend>roscpp</build_export_depend> <build_export_depend>roscpp</build_export_depend>
<build_export_depend>std_msgs</build_export_depend> <build_export_depend>std_msgs</build_export_depend>
<build_export_depend>gazebo_ros</build_export_depend> <build_export_depend>gazebo_ros</build_export_depend>
<exec_depend>roscpp</exec_depend> <exec_depend>roscpp</exec_depend>
<exec_depend>std_msgs</exec_depend> <exec_depend>std_msgs</exec_depend>
<exec_depend>gazebo_ros</exec_depend> <exec_depend>gazebo_ros</exec_depend>
<exec_depend>message_runtime</exec_depend>
<!-- The export tag contains other, unspecified, tags --> <!-- The export tag contains other, unspecified, tags -->

26
rrrobot_src/src/simulation_drivers/src/simulation_env/src/arm_angle_sensor_plugin.cpp
View File

@@ -5,6 +5,7 @@
#include <iostream> #include <iostream>
#include "ros/ros.h" #include "ros/ros.h"
#include "simulation_env/arm_angles.h"
using std::cout; using std::cout;
using std::endl; using std::endl;
@@ -29,7 +30,7 @@ namespace gazebo
} }
nh.reset(new ros::NodeHandle("arm_node")); nh.reset(new ros::NodeHandle("arm_node"));
publisher = nh->advertise<std_msgs::Float32>("arm_positions", 1); publisher = nh->advertise<simulation_env::arm_angles>("arm_positions", 1);
} }
private: private:
@@ -37,18 +38,25 @@ namespace gazebo
void onUpdate() void onUpdate()
{ {
//cout << __func__ << endl; //cout << __func__ << endl;
std_msgs::Float32 msg; simulation_env::arm_angles msg;
msg.data = 0.0f; //msg.data = 0.0f;
gazebo::physics::BasePtr joint = model->GetChild("shoulder_joint"); msg.shoulder_pivot_angle = dynamic_cast<gazebo::physics::Joint*>(model->GetChild("shoulder_pivot").get())->Position();
msg.shoulder_joint_angle = dynamic_cast<gazebo::physics::Joint*>(model->GetChild("shoulder_joint").get())->Position();
msg.elbow_joint_angle = dynamic_cast<gazebo::physics::Joint*>(model->GetChild("elbow_joint").get())->Position();
msg.wrist_pivot_angle = dynamic_cast<gazebo::physics::Joint*>(model->GetChild("wrist_pivot").get())->Position();
msg.wrist_joint_angle = dynamic_cast<gazebo::physics::Joint*>(model->GetChild("wrist_joint").get())->Position();
msg.end_effector_pivot_angle = dynamic_cast<gazebo::physics::Joint*>(model->GetChild("end_effector_pivot").get())->Position();
//gazebo::physics::BasePtr joint = model->GetChild("shoulder_joint");
//cout << "Joint type: " << joint->GetType() << "joint type & EntityType::JOINT = " << (joint->GetType() & gazebo::physics::Base::EntityType::JOINT) << endl; //cout << "Joint type: " << joint->GetType() << "joint type & EntityType::JOINT = " << (joint->GetType() & gazebo::physics::Base::EntityType::JOINT) << endl;
if((joint->GetType() & gazebo::physics::Base::EntityType::JOINT) != 0) //if((joint->GetType() & gazebo::physics::Base::EntityType::JOINT) != 0)
{ //{
//cout << "Got an element of type JOINT" << endl; //cout << "Got an element of type JOINT" << endl;
//cout << "Joint Position: " << dynamic_cast<gazebo::physics::Joint*>(joint.get())->Position() << endl; //cout << "Joint Position: " << dynamic_cast<gazebo::physics::Joint*>(joint.get())->Position() << endl;
msg.data = dynamic_cast<gazebo::physics::Joint*>(joint.get())->Position(); //msg.data = dynamic_cast<gazebo::physics::Joint*>(joint.get())->Position();
//cout << "Position: " << msg.data << endl; //cout << "Position: " << msg.data << endl;
} //}
if(msg.data != 0.0f) publisher.publish(msg); /*if(msg.data != 0.0f) */publisher.publish(msg);
ros::spinOnce(); ros::spinOnce();
} }
physics::ModelPtr model; physics::ModelPtr model;